From eb47a582ce210b81234c17c182345605dc4e2d85 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Mon, 25 Mar 2024 20:48:02 -0400 Subject: [PATCH 1/2] [#495] - combined point at point w/ point at tag --- src/main/java/frc/robot/Constants.java | 8 +- src/main/java/frc/robot/RobotContainer.java | 5 +- .../java/frc/robot/command/ComboPoint.java | 208 ++++++++++++++++++ .../java/frc/robot/command/PointAtPoint.java | 2 +- 4 files changed, 218 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/command/ComboPoint.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b244294e..3827c2f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -321,8 +321,10 @@ public class VisionConstants { public static final Translation2d VISION_LIMIT = new Translation2d(Units.feetToMeters(9), Units.feetToMeters(5)); public static final double ALIGNMENT_TOLERANCE = 8d; // TODO: make this an actual value - public static final double POINTATTAG_ALIGNMENT_TOLERANCE = 2d; - public static final PIDController TAG_AIM_CONTROLLER = new PIDController(0.2, 0, 0.015, 0.01); + public static final double POINTATTAG_ALIGNMENT_TOLERANCE = 1d; + public static final PIDController POINT_AIM_CONTROLLER = new PIDController(0.2, 0, 0.015, 0.01); + public static final PIDController TAG_AIM_CONTROLLER = new PIDController(0.1, 0, 0.01); + public static final PIDController COMBO_CONTROLLER = new PIDController(0.1, 0, 0); public static final PIDController CHASE_CONTROLLER = new PIDController(0.05, 0, 0); public static final int TAG_PIPELINE = 0; public static final int SPEAKER_PIPELINE = 1; @@ -681,7 +683,7 @@ public class ClimbConstants { // TODO: find real values public class LEDsConstants { - public static final int LED_LENGTH = 29; + public static final int LED_LENGTH = 50; public static final Map STRAND_START = new HashMap() { { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f1b30585..1e23ae1c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ import frc.robot.command.Collect; import frc.robot.command.CollectAndGo; import frc.robot.command.CollisionDetection; +import frc.robot.command.ComboPoint; import frc.robot.command.HasPieceAuto; import frc.robot.command.Index; import frc.robot.command.ManualClimb; @@ -233,8 +234,10 @@ protected void configureButtonBindings() { new Trigger(driver::getYButton) .whileTrue(new PointAtTag(drivetrain, limelights, driver)); // TODO: make work + // new Trigger(driver::getLeftBumper) + // .whileTrue(new PointAtPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver)); new Trigger(driver::getLeftBumper) - .whileTrue(new PointAtPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver)); + .whileTrue(new ComboPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver, limelights)); // new Trigger(driver::getYButton) // .whileTrue(new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain)); diff --git a/src/main/java/frc/robot/command/ComboPoint.java b/src/main/java/frc/robot/command/ComboPoint.java new file mode 100644 index 00000000..ee24df2a --- /dev/null +++ b/src/main/java/frc/robot/command/ComboPoint.java @@ -0,0 +1,208 @@ +// 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 frc.robot.command; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DrivetrainConstants; +import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.Limelights; +import frc.robot.subsystems.Swerve; +import frc.thunder.shuffleboard.LightningShuffleboard; +import frc.thunder.vision.Limelight; + +public class ComboPoint extends Command { + private double minPower = 0.3; + + private Swerve drivetrain; + private XboxController driver; + private Limelight stopMe; + + private double pidOutput; + private double targetHeading; + private Translation2d targetPose; + private Translation2d originalTargetPose; + + private PIDController pointController = VisionConstants.TAG_AIM_CONTROLLER; + private PIDController tagController = VisionConstants.COMBO_CONTROLLER; + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.2, 0.5); + + private Debouncer debouncer = new Debouncer(0.2); + + private DoubleLogEntry deltaYLog; + private DoubleLogEntry deltaXLog; + private DoubleLogEntry targetHeadingLog; + private DoubleLogEntry targetYLog; + private DoubleLogEntry targetXLog; + private DoubleLogEntry pidOutputLog; + private DoubleLogEntry targetMinusCurrentHeadingLog; + private DoubleLogEntry currentLog; + private BooleanLogEntry inToleranceLog; + + /** + * Creates a new ComboPoint. + * + * @param targetPose the target pose to point at + * @param drivetrain to request movement + * @param driver the driver's controller, used for drive input + */ + public ComboPoint(Translation2d targetPose, Swerve drivetrain, XboxController driver, Limelights limelights) { + this.drivetrain = drivetrain; + this.driver = driver; + this.originalTargetPose = targetPose; + this.stopMe = limelights.getStopMe(); + + addRequirements(drivetrain); + + initLogging(); + } + + public ComboPoint(double targetX, double targetY, Swerve drivetrain, XboxController driver, Limelights limelights) { + this(new Translation2d(targetX, targetY), drivetrain, driver, limelights); + } + + private boolean isBlueAlliance() { + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() && alliance.get() == Alliance.Blue; + } + + private Translation2d swapAlliance(Translation2d pose) { + return new Translation2d(VisionConstants.FIELD_LIMIT.getX() - pose.getX(), pose.getY()); + } + + private boolean inTolerance() { + return Math.abs(targetHeading - drivetrain.getPose().getRotation().getDegrees()) + % 360 < DrivetrainConstants.ALIGNMENT_TOLERANCE; + } + + private boolean inTagTolerance() { + return Math.abs(targetHeading) < VisionConstants.POINTATTAG_ALIGNMENT_TOLERANCE && stopMe.hasTarget(); + } + + @Override + public void initialize() { + pointController.enableContinuousInput(0, 360); + pointController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); + tagController.enableContinuousInput(0, 360); + tagController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); + if (isBlueAlliance()) { + targetPose = originalTargetPose; + } else { + targetPose = swapAlliance(originalTargetPose); + } + + stopMe.setPipeline(VisionConstants.SPEAKER_PIPELINE); + + System.out.println("DRIVE - COMBO POINT START"); + } + + /** + * update logging + */ + public void initLogging() { + DataLog log = DataLogManager.getLog(); + + deltaYLog = new DoubleLogEntry(log, "/ComboPoint/Delta Y"); + deltaXLog = new DoubleLogEntry(log, "/ComboPoint/Delta X"); + targetHeadingLog = new DoubleLogEntry(log, "/ComboPoint/Target Heading"); + targetYLog = new DoubleLogEntry(log, "/ComboPoint/Target Pose Y"); + targetXLog = new DoubleLogEntry(log, "/ComboPoint/Target Pose X"); + pidOutputLog = new DoubleLogEntry(log, "/ComboPoint/Pid Output"); + targetMinusCurrentHeadingLog = new DoubleLogEntry(log, "/ComboPoint/target minus current heading"); + currentLog = new DoubleLogEntry(log, "/ComboPoint/Current"); + inToleranceLog = new BooleanLogEntry(log, "/ComboPoint/InTolerance"); + } + + + public void setDebugging(){ + pointController.setP(LightningShuffleboard.getDouble("ComboPoint", "P", pointController.getP())); + pointController.setI(LightningShuffleboard.getDouble("ComboPoint", "I", pointController.getI())); + pointController.setD(LightningShuffleboard.getDouble("ComboPoint", "D", pointController.getD())); + minPower = LightningShuffleboard.getDouble("ComboPoint", "Min Power", minPower); + } + + @Override + public void execute() { + Pose2d pose = drivetrain.getPose(); + var deltaX = targetPose.getX() - pose.getX(); + var deltaY = targetPose.getY() - pose.getY(); + + + if (stopMe.hasTarget() && stopMe.getPipeline() == VisionConstants.SPEAKER_PIPELINE) { + targetHeading = stopMe.getTargetX(); + if (inTagTolerance()){ + pidOutput = 0d; + } else { + double currentHeading = (pose.getRotation().getDegrees() + 360) % 360; + pidOutput = tagController.calculate(currentHeading, currentHeading - targetHeading); + } + } else { + targetHeading = Math.toDegrees(Math.atan2(deltaY, deltaX)) + 360 + 180; + targetHeading %= 360; + + pidOutput = pointController.calculate((pose.getRotation().getDegrees() + 360) % 360, targetHeading); + // if (!inTolerance() && Math.abs(pidOutput) < minPower) { + // pidOutput = Math.signum(pidOutput) * minPower; + // } + } + + double feedForwardOutput = feedforward.calculate(pidOutput); + + // setDebugging(); + + drivetrain.setField(-driver.getLeftY(), -driver.getLeftX(), feedForwardOutput); + + LightningShuffleboard.setDouble("ComboPoint", "Target Heading", targetHeading); + LightningShuffleboard.setDouble("ComboPoint", "Current Heading", + drivetrain.getPose().getRotation().getDegrees()); + LightningShuffleboard.setBool("ComboPoint", "In Tolerance", inTolerance()); + LightningShuffleboard.setDouble("ComboPoint", "Raw Output (PointController)", pidOutput); + LightningShuffleboard.setDouble("ComboPoint", "Output (FeedForward)", feedForwardOutput); + LightningShuffleboard.setBool("ComboPoint", "HasTarget", stopMe.hasTarget()); + + updateLogging(); + } + + /** + * update logging + */ + public void updateLogging() { + deltaYLog.append(targetPose.getY() - drivetrain.getPose().getY()); + deltaXLog.append(targetPose.getX() - drivetrain.getPose().getX()); + targetHeadingLog.append(targetHeading); + targetYLog.append(targetPose.getY()); + targetXLog.append(targetPose.getX()); + pidOutputLog.append(pidOutput); + targetMinusCurrentHeadingLog.append(Math.abs(targetHeading - drivetrain.getPose().getRotation().getDegrees())); + currentLog.append(drivetrain.getPose().getRotation().getDegrees()); + inToleranceLog.append(inTolerance()); + } + + @Override + public void end(boolean interrupted) { + System.out.println("DRIVE - COMBO POINT END"); + stopMe.setPipeline(VisionConstants.TAG_PIPELINE); + } + + @Override + public boolean isFinished() { + if (DriverStation.isAutonomous()) { + return debouncer.calculate(inTagTolerance()); + } + return debouncer.calculate(inTagTolerance()); + } +} diff --git a/src/main/java/frc/robot/command/PointAtPoint.java b/src/main/java/frc/robot/command/PointAtPoint.java index 30fea74f..34b40bce 100644 --- a/src/main/java/frc/robot/command/PointAtPoint.java +++ b/src/main/java/frc/robot/command/PointAtPoint.java @@ -28,7 +28,7 @@ public class PointAtPoint extends Command { private Translation2d targetPose; private Translation2d originalTargetPose; - private PIDController headingController = VisionConstants.TAG_AIM_CONTROLLER; + private PIDController headingController = VisionConstants.POINT_AIM_CONTROLLER; private DoubleLogEntry deltaYLog; private DoubleLogEntry deltaXLog; From 44bb581261f85414450c47c6ab875d6f3a1dd0c3 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Mon, 25 Mar 2024 20:52:05 -0400 Subject: [PATCH 2/2] [#495] - fixed errors --- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/command/ComboPoint.java | 7 ++++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3f387825..8bbeb2ef 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -337,7 +337,6 @@ public class Pipelines { // TODO get real public static final int TAG_PIPELINE = 0; public static final int SPEAKER_PIPELINE = 1; public static final int NOTE_PIPELINE = 2; - } } diff --git a/src/main/java/frc/robot/command/ComboPoint.java b/src/main/java/frc/robot/command/ComboPoint.java index ee24df2a..077b48fb 100644 --- a/src/main/java/frc/robot/command/ComboPoint.java +++ b/src/main/java/frc/robot/command/ComboPoint.java @@ -59,6 +59,7 @@ public class ComboPoint extends Command { * @param targetPose the target pose to point at * @param drivetrain to request movement * @param driver the driver's controller, used for drive input + * @param limelights for tag align */ public ComboPoint(Translation2d targetPose, Swerve drivetrain, XboxController driver, Limelights limelights) { this.drivetrain = drivetrain; @@ -105,7 +106,7 @@ public void initialize() { targetPose = swapAlliance(originalTargetPose); } - stopMe.setPipeline(VisionConstants.SPEAKER_PIPELINE); + stopMe.setPipeline(VisionConstants.Pipelines.SPEAKER_PIPELINE); System.out.println("DRIVE - COMBO POINT START"); } @@ -142,7 +143,7 @@ public void execute() { var deltaY = targetPose.getY() - pose.getY(); - if (stopMe.hasTarget() && stopMe.getPipeline() == VisionConstants.SPEAKER_PIPELINE) { + if (stopMe.hasTarget() && stopMe.getPipeline() == VisionConstants.Pipelines.SPEAKER_PIPELINE) { targetHeading = stopMe.getTargetX(); if (inTagTolerance()){ pidOutput = 0d; @@ -195,7 +196,7 @@ public void updateLogging() { @Override public void end(boolean interrupted) { System.out.println("DRIVE - COMBO POINT END"); - stopMe.setPipeline(VisionConstants.TAG_PIPELINE); + stopMe.setPipeline(VisionConstants.Pipelines.TAG_PIPELINE); } @Override