diff --git a/photonvision-settings-export.zip b/photonvision-settings-export.zip new file mode 100644 index 0000000..ceeb004 Binary files /dev/null and b/photonvision-settings-export.zip differ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fef8337..65a1f60 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,11 +15,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; @@ -31,8 +28,8 @@ import frc.robot.subsystems.Intake; import frc.robot.subsystems.Led; import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.Undertaker; import frc.robot.subsystems.Shooter.Speeds; +import frc.robot.subsystems.Undertaker; public class Robot extends CommandRobot { @@ -131,9 +128,7 @@ public void disabledInit() { @Override public void configureButtonBindings() { - driveController.back().onTrue(drive.resetGyroCommand()); - // Magic numbers for auto testing - driveController.start().onTrue(drive.setPoseCommand(() -> drive.visionEstimator.getEstimatedPosition())); + driveController.back().onTrue(drive.resetCmd()); driveController.leftBumper() .whileTrue(drive.robotCentricDrive(() -> { return driveScaler.applyAsDouble(-driveController.getLeftX()); diff --git a/src/main/java/frc/robot/subsystems/ArmRotate.java b/src/main/java/frc/robot/subsystems/ArmRotate.java index 9acfec1..5fca44f 100644 --- a/src/main/java/frc/robot/subsystems/ArmRotate.java +++ b/src/main/java/frc/robot/subsystems/ArmRotate.java @@ -9,7 +9,6 @@ import com.chopshop166.chopshoplib.motors.Modifier; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; @@ -19,11 +18,7 @@ public class ArmRotate extends LoggedSubsystem { - // Intake Position: 0 degrees <- base everything else off of it - final ProfiledPIDController pid; - // Set to zero until able to test - private boolean useAbsolute = true; final double RAISE_SPEED = .85; final double MANUAL_LOWER_SPEED_COEF = 0.5; final double SLOW_DOWN_COEF = 0.5; @@ -32,7 +27,6 @@ public class ArmRotate extends LoggedSubsystem { final double INTAKE_TOLERANCE = 4; double holdAngle = 0; - private Constraints rotateConstraints = new Constraints(150, 200); ArmPresets level = ArmPresets.OFF; public ArmRotate(ArmRotateMap armRotateMap) { diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index e58253d..f12ca9f 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -9,7 +9,6 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonTrackedTarget; import com.chopshop166.chopshoplib.logging.LoggedSubsystem; import com.chopshop166.chopshoplib.logging.data.SwerveDriveData; @@ -51,12 +50,10 @@ public class Drive extends LoggedSubsystem { double rotationCoef = 1; double rotationKp = 0.05; double rotationKs = 0.19; - ProfiledPIDController rotationPID = new ProfiledPIDController(0.09, 0.0, 0.0, new Constraints(240, 270)); + ProfiledPIDController rotationPID = new ProfiledPIDController(0.065, 0.0, 0.0, new Constraints(240, 270)); double visionMaxError = 1; - Optional tgt = Optional.empty(); SwerveDrivePoseEstimator estimator; - public SwerveDrivePoseEstimator visionEstimator; // Vision objects private PhotonCamera camera = null; @@ -66,8 +63,8 @@ public class Drive extends LoggedSubsystem { // Cam mounted facing forward, half a meter forward of center, half a meter up // from center. public static final Transform3d kRobotToCam = new Transform3d( - new Translation3d(Units.inchesToMeters(3.029), Units.inchesToMeters(-6.9965), Units.inchesToMeters(12.445)), - new Rotation3d(0, Units.degreesToRadians(-16.875), Units.degreesToRadians(-4.5 - 180))); + new Translation3d(Units.inchesToMeters(-6.9965), Units.inchesToMeters(-3.029), Units.inchesToMeters(12.445)), + new Rotation3d(0, Units.degreesToRadians(-16.875), Units.degreesToRadians(-6.5))); // The layout of the AprilTags on the field public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); @@ -93,11 +90,6 @@ public Drive(SwerveDriveMap map) { new Pose2d(), VecBuilder.fill(0.02, 0.02, 0.01), VecBuilder.fill(0.1, 0.1, 0.01)); - visionEstimator = new SwerveDrivePoseEstimator(kinematics, getMap().gyro.getRotation2d(), - getData().getModulePositions(), - new Pose2d(), - VecBuilder.fill(0.02, 0.02, 0.01), - VecBuilder.fill(0.1, 0.1, 0.01)); AutoBuilder.configureHolonomic(estimator::getEstimatedPosition, this::setPose, this::getSpeeds, this::move, // Method that will drive the robot given ROBOT @@ -116,8 +108,6 @@ public Drive(SwerveDriveMap map) { public void setPose(Pose2d pose) { estimator.resetPosition(getMap().gyro.getRotation2d(), getData().getModulePositions(), pose); - visionEstimator.resetPosition(getMap().gyro.getRotation2d(), - getData().getModulePositions(), pose); } public Command setPoseCommand(Supplier pose) { @@ -129,7 +119,7 @@ public Command setPoseCommand(Supplier pose) { private void deadbandMove(final double xSpeed, final double ySpeed, final double rotation, boolean isRobotCentric) { - var deadband = Modifier.scalingDeadband(DriverStation.isFMSAttached() ? 0.05 : 0.15); + var deadband = Modifier.scalingDeadband(0.05); double rotationInput = deadband.applyAsDouble(rotation); double xInput = deadband.applyAsDouble(xSpeed); double yInput = deadband.applyAsDouble(ySpeed); @@ -185,10 +175,6 @@ public Command robotCentricDrive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, D .withName("Robot Centric Drive"); } - public Command resetGyroCommand() { - return runOnce(this::resetGyro).ignoringDisable(true); - } - public Command moveInDirection(double xSpeed, double ySpeed, double seconds) { return run(() -> { move(xSpeed, ySpeed, 0, false); @@ -197,62 +183,45 @@ public Command moveInDirection(double xSpeed, double ySpeed, double seconds) { public Command rotateToAngle(double targetAngle) { return run(() -> { - double newTarget = targetAngle; - if (Math.abs(getMap().gyro.getRotation2d().getDegrees() - targetAngle) > 180) { - newTarget -= 360; - } - double rotationSpeed = rotationPID.calculate(getMap().gyro.getRotation2d().getDegrees(), newTarget); - rotationSpeed += Math.copySign(rotationKs, rotationSpeed); - // need to ensure we move at a fast enough speed for gyro to keep up - if (Math.abs(rotationSpeed) > 0.2 && Math.abs(rotationPID.getPositionError()) > 0.75) { - move(0, 0, rotationSpeed, - false); - } else { - safeState(); - } - Logger.recordOutput("rotationSpeed", rotationSpeed); - Logger.recordOutput("targetVelocity", rotationPID.getSetpoint().velocity); + rotateToAngleImpl(targetAngle); }); } public Translation2d getSpeakerTarget() { Optional pose; if (isBlue) { - pose = photonEstimator.getFieldTags().getTagPose(7); + Logger.recordOutput("Alliance Speaker", "Blue"); + pose = kTagLayout.getTagPose(7); } else { - pose = photonEstimator.getFieldTags().getTagPose(4); + Logger.recordOutput("Alliance Speaker", "Red/Other"); + pose = kTagLayout.getTagPose(4); } if (pose.isEmpty()) { return new Translation2d(); } - return new Translation2d(pose.get().getTranslation().getX(), pose.get().getTranslation().getY()); + return pose.get().getTranslation().toTranslation2d(); } public Translation2d getRobotToTarget(Translation2d target) { - return target.minus(visionEstimator.getEstimatedPosition().getTranslation()); + return target.minus(estimator.getEstimatedPosition().getTranslation()); } public Command rotateToSpeaker() { + return rotateTo(this::getSpeakerTarget); + } + + public Command rotateTo(Supplier target) { return run(() -> { - var target = getRobotToTarget(getSpeakerTarget()); - double targetAngle = target.getAngle().getDegrees(); - double estimatorAngle = estimator.getEstimatedPosition().getRotation().getDegrees(); - Logger.recordOutput("Estimator Angle", estimatorAngle); - Logger.recordOutput("adjustedTargetAngle", targetAngle); - double rotationSpeed = rotationPID.calculate(estimatorAngle, - targetAngle); - rotationSpeed += Math.copySign(rotationKs, rotationSpeed); - // need to ensure we move at a fast enough speed for gyro to keep up - if (Math.abs(rotationSpeed) > 0.2 && Math.abs(rotationPID.getPositionError()) > 0.75) { - move(0, 0, rotationSpeed, - false); - } else { - safeState(); - } - Logger.recordOutput("Speaker Pose", getSpeakerTarget()); + var robotToTarget = getRobotToTarget(target.get()); + rotateToAngleImpl(robotToTarget.getAngle().getDegrees()); + Logger.recordOutput("Target Pose", robotToTarget); }); } + public Command rotateTo(Translation2d target) { + return rotateTo(() -> target); + } + /** * The latest estimated robot pose on the field from vision data. This may be * empty. This should @@ -279,7 +248,7 @@ public Matrix getEstimationStdDevs(Pose2d estimatedPose) { int numTags = 0; double avgDist = 0; for (var tgt : targets) { - var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + var tagPose = kTagLayout.getTagPose(tgt.getFiducialId()); if (tagPose.isEmpty()) continue; numTags++; @@ -302,7 +271,7 @@ public Matrix getEstimationStdDevs(Pose2d estimatedPose) { @Override public void reset() { - // Nothing to reset here + getMap().gyro.reset(); } @Override @@ -317,31 +286,36 @@ public void periodic() { super.periodic(); isBlue = DriverStation.getAlliance().orElse(Alliance.Red) == Alliance.Blue; estimator.update(getMap().gyro.getRotation2d(), getData().getModulePositions()); - visionEstimator.update(getMap().gyro.getRotation2d(), getData().getModulePositions()); - var target = getRobotToTarget(getSpeakerTarget()); - double targetAngle = target.getAngle().getDegrees(); // Correct pose estimate with vision measurements var visionEst = getEstimatedGlobalPose(); - visionEst.ifPresent( - est -> { - var estPose = est.estimatedPose.toPose2d(); - // Change our trust in the measurement based on the tags we can see - var estStdDevs = getEstimationStdDevs(estPose); - Logger.recordOutput("Vision Pose", est.estimatedPose); - visionEstimator.addVisionMeasurement( - est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs); - }); - - Logger.recordOutput("estimatorPose", estimator.getEstimatedPosition()); - Logger.recordOutput("visionEstimatorPose", visionEstimator.getEstimatedPosition()); - Logger.recordOutput("poseAngle", estimator.getEstimatedPosition().getRotation()); - Logger.recordOutput("robotRotationGyro", getMap().gyro.getRotation2d()); - Logger.recordOutput("targetAngle", targetAngle); + visionEst.ifPresent(est -> { + var estPose = est.estimatedPose.toPose2d(); + // Change our trust in the measurement based on the tags we can see + var estStdDevs = getEstimationStdDevs(estPose); + Logger.recordOutput("Vision Pose", est.estimatedPose); + estimator.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs); + }); + Logger.recordOutput("Estimator Pose", estimator.getEstimatedPosition()); + Logger.recordOutput("Pose Angle", estimator.getEstimatedPosition().getRotation()); + Logger.recordOutput("Robot Rotation Gyro", getMap().gyro.getRotation2d()); } - public void resetGyro() { - getMap().gyro.reset(); + private void rotateToAngleImpl(double targetAngle) { + double estimatorAngle = estimator.getEstimatedPosition().getRotation().getDegrees(); + double rotationSpeed = rotationPID.calculate(estimatorAngle, targetAngle); + rotationSpeed += Math.copySign(rotationKs, rotationSpeed); + // need to ensure we move at a fast enough speed for gyro to keep up + if (Math.abs(rotationSpeed) > 0.2 && Math.abs(rotationPID.getPositionError()) > 0.75) { + move(0, 0, rotationSpeed, false); + } else { + safeState(); + } + Logger.recordOutput("Target Angle", targetAngle); + Logger.recordOutput("Estimator Angle", estimatorAngle); + Logger.recordOutput("Rotation Speed", rotationSpeed); + Logger.recordOutput("Target Velocity", rotationPID.getSetpoint().velocity); + Logger.recordOutput("Position Error", rotationPID.getPositionError()); } } diff --git a/src/main/java/frc/robot/subsystems/Led.java b/src/main/java/frc/robot/subsystems/Led.java index 6609e96..42a00bf 100644 --- a/src/main/java/frc/robot/subsystems/Led.java +++ b/src/main/java/frc/robot/subsystems/Led.java @@ -2,7 +2,6 @@ import com.chopshop166.chopshoplib.leds.LEDSubsystem; import com.chopshop166.chopshoplib.leds.patterns.AlliancePattern; -import com.chopshop166.chopshoplib.leds.patterns.ColdFirePattern; import com.chopshop166.chopshoplib.leds.patterns.FlashPattern; import com.chopshop166.chopshoplib.leds.patterns.RainbowRoad; import com.chopshop166.chopshoplib.leds.patterns.SpinPattern;