From e26a4365532cc18a340e12e66584d2c76272bbd3 Mon Sep 17 00:00:00 2001 From: vilok1 Date: Mon, 11 Mar 2024 19:27:13 -0400 Subject: [PATCH 01/38] [#422] delete obsolete command --- .../java/frc/robot/command/ManualClimb.java | 42 ------------------- 1 file changed, 42 deletions(-) delete mode 100644 src/main/java/frc/robot/command/ManualClimb.java diff --git a/src/main/java/frc/robot/command/ManualClimb.java b/src/main/java/frc/robot/command/ManualClimb.java deleted file mode 100644 index ef9c99ea..00000000 --- a/src/main/java/frc/robot/command/ManualClimb.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.command; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Climber; - -public class ManualClimb extends Command { - - private Climber climber; - private DoubleSupplier leftPower; - private DoubleSupplier rightPower; - - /** - * Creates a new ManualClimb. - * @param leftPower supplier for left motor power - * @param rightPower supplier for right motor power - * @param climber subsystem - */ - public ManualClimb(DoubleSupplier leftPower, DoubleSupplier rightPower, Climber climber) { - this.climber = climber; - this.leftPower = leftPower; - this.rightPower = rightPower; - - addRequirements(climber); - } - - @Override - public void execute() { - climber.setPower(leftPower.getAsDouble(), rightPower.getAsDouble()); - } - - @Override - public void end(boolean interrupted) { - climber.stop(); - } - - @Override - public boolean isFinished() { - return false; - } -} From ea91524157104adc1b51840ddb7897e3c438d044 Mon Sep 17 00:00:00 2001 From: vilok1 Date: Mon, 11 Mar 2024 19:27:35 -0400 Subject: [PATCH 02/38] [#422] remove unnececary requirements --- src/main/java/frc/robot/command/PathFindToAuton.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/command/PathFindToAuton.java b/src/main/java/frc/robot/command/PathFindToAuton.java index c28037da..f5f5f05f 100644 --- a/src/main/java/frc/robot/command/PathFindToAuton.java +++ b/src/main/java/frc/robot/command/PathFindToAuton.java @@ -23,7 +23,6 @@ public class PathFindToAuton extends Command { private Swerve drivetrain; - private XboxControllerFilter controller; // Driver Controller private AutoBuilder autoBuilder; private Command pathFindCommand; private PathPlannerPath autonPath; @@ -32,11 +31,9 @@ public class PathFindToAuton extends Command { * Pathfinds to a specific pose given * @param pathfindingPose The pose to pathfind to * @param drivetrain The drivetrain subsystem - * @param controller The driver controller */ - public PathFindToAuton(PathPlannerPath autonPath, Swerve drivetrain, XboxControllerFilter controller) { + public PathFindToAuton(PathPlannerPath autonPath, Swerve drivetrain) { this.drivetrain = drivetrain; - this.controller = controller; this.autonPath = autonPath; autoBuilder = new AutoBuilder(); From 3fb556ccb2ffe55c57de82f0bb01e5133c64c938 Mon Sep 17 00:00:00 2001 From: vilok1 Date: Mon, 11 Mar 2024 19:28:10 -0400 Subject: [PATCH 03/38] [#422] auto lineup when a button pressed --- src/main/java/frc/robot/Constants.java | 6 +-- .../java/frc/robot/command/SmartClimb.java | 50 +++++++++++++++++++ 2 files changed, 53 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ae1bf5c5..f2220665 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -593,9 +593,9 @@ public class ClimbConstants { // TODO: find real values public static final double CLIMB_SYSTEST_POWER = 0.1; - public enum CLIMBER_STATES { - CLIMBED, GROUNDED, STOW - } + public static final Pose2d PATHFIND_CENTER_STAGE_START_POSE = new Pose2d(7.43, 4.16, new Rotation2d(Math.PI)); + public static final Pose2d PATHFIND_HIGH_STAGE_START_POSE = new Pose2d(3.55, 6.16, new Rotation2d(Units.degreesToRadians(-53.13))); + public static final Pose2d PATHFIND_LOW_STAGE_START_POSE = new Pose2d(3.54, 1.91, new Rotation2d(Units.degreesToRadians(63.43))); } public class LEDsConstants { diff --git a/src/main/java/frc/robot/command/SmartClimb.java b/src/main/java/frc/robot/command/SmartClimb.java index 6963ad93..d2892e7b 100644 --- a/src/main/java/frc/robot/command/SmartClimb.java +++ b/src/main/java/frc/robot/command/SmartClimb.java @@ -1,9 +1,13 @@ package frc.robot.command; import java.util.function.DoubleSupplier; + +import com.pathplanner.lib.path.PathPlannerPath; + import java.util.function.BooleanSupplier; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.ClimbConstants; import frc.robot.subsystems.Climber; import frc.robot.subsystems.Swerve; @@ -11,6 +15,7 @@ public class SmartClimb extends Command { private Climber climber; private Swerve drivetrain; + private PathFindToAuton pathFind; private DoubleSupplier leftPower; private DoubleSupplier rightPower; @@ -18,6 +23,7 @@ public class SmartClimb extends Command { private boolean buttonState; private boolean autoClimbEngaged; + private boolean readyToLineUp; /** * SmartClimb to control the climber using the B button and sticks @@ -54,6 +60,12 @@ public void execute() { } } else if (bButton.getAsBoolean() && !autoClimbEngaged) { // Auto deploy climb when B button is pressed and auto climb has not been engaged + if (readyToLineUp) { + lineUp(); + } + if (pathFind.isFinished()) { + readyToLineUp = true; + } climber.deploy(); buttonState = true; } @@ -68,4 +80,42 @@ public void end(boolean interrupted) { public boolean isFinished() { return false; } + + public Command lineUp() { + readyToLineUp = false; + + // Determine which path to take based on the robot's current position + double diffCenterX = Math.abs(drivetrain.getPose().getTranslation().getX() + - ClimbConstants.PATHFIND_CENTER_STAGE_START_POSE.getTranslation().getX()); + double diffCenterY = Math.abs(drivetrain.getPose().getTranslation().getY() + - ClimbConstants.PATHFIND_CENTER_STAGE_START_POSE.getTranslation().getY()); + double diffCenter = Math.hypot(diffCenterX, diffCenterY); + + double diffHighX = Math.abs(drivetrain.getPose().getTranslation().getX() + - ClimbConstants.PATHFIND_HIGH_STAGE_START_POSE.getTranslation().getX()); + double diffHighY = Math.abs(drivetrain.getPose().getTranslation().getY() + - ClimbConstants.PATHFIND_HIGH_STAGE_START_POSE.getTranslation().getY()); + double diffHigh = Math.hypot(diffHighX, diffHighY); + + double diffLowX = Math.abs(drivetrain.getPose().getTranslation().getX() + - ClimbConstants.PATHFIND_LOW_STAGE_START_POSE.getTranslation().getX()); + double diffLowY = Math.abs(drivetrain.getPose().getTranslation().getY() + - ClimbConstants.PATHFIND_LOW_STAGE_START_POSE.getTranslation().getY()); + double diffLow = Math.hypot(diffLowX, diffLowY); + + // Create the pathfind command based on the shortest distance + if (diffCenter < diffHigh && diffCenter < diffLow) { + pathFind = new PathFindToAuton( + PathPlannerPath.fromPathFile("Pathfind-CENTER-STAGE"), drivetrain); + } else if (diffHigh < diffCenter && diffHigh < diffLow) { + pathFind = new PathFindToAuton( + PathPlannerPath.fromPathFile("Pathfind-HIGH-STAGE"), drivetrain); + } else { + pathFind = new PathFindToAuton( + PathPlannerPath.fromPathFile("Pathfind-LOW-STAGE"), drivetrain); + } + + pathFind.schedule(); + return pathFind; + } } From c55a30159df9245a416bc513722f4e53c503c0ca Mon Sep 17 00:00:00 2001 From: vilok1 Date: Mon, 11 Mar 2024 19:32:46 -0400 Subject: [PATCH 04/38] [#422] add javadocs --- src/main/java/frc/robot/command/SmartClimb.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/command/SmartClimb.java b/src/main/java/frc/robot/command/SmartClimb.java index d2892e7b..47486d84 100644 --- a/src/main/java/frc/robot/command/SmartClimb.java +++ b/src/main/java/frc/robot/command/SmartClimb.java @@ -51,6 +51,7 @@ public void execute() { // Engage Manual Climb whenever sticks are active climber.setPower(leftPower.getAsDouble(), rightPower.getAsDouble()); autoClimbEngaged = false; + readyToLineUp = true; } else if ((buttonState && !bButton.getAsBoolean()) || (drivetrain.isTipped() && !autoClimbEngaged)) { // Auto retract on the falling edge of the B button or if the robot is tipped climber.retract(); @@ -59,13 +60,11 @@ public void execute() { autoClimbEngaged = true; // trigger autoClimbEngaged to prevent auto deploy from re-engaging } } else if (bButton.getAsBoolean() && !autoClimbEngaged) { - // Auto deploy climb when B button is pressed and auto climb has not been engaged + // line up if not currently lining up if (readyToLineUp) { lineUp(); } - if (pathFind.isFinished()) { - readyToLineUp = true; - } + // Auto deploy climb when B button is pressed and auto climb has not been engaged climber.deploy(); buttonState = true; } @@ -81,7 +80,10 @@ public boolean isFinished() { return false; } - public Command lineUp() { + /** + * Line up the robot to climb + */ + public void lineUp() { readyToLineUp = false; // Determine which path to take based on the robot's current position @@ -116,6 +118,5 @@ public Command lineUp() { } pathFind.schedule(); - return pathFind; } } From e61dc6746e91604542ceb2241f9efdc07518ff59 Mon Sep 17 00:00:00 2001 From: vilok1 Date: Mon, 11 Mar 2024 19:33:21 -0400 Subject: [PATCH 05/38] [#422] fix error --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0b8a2202..ae290b85 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,7 +35,6 @@ import frc.robot.command.PathFindToAuton; import frc.robot.command.PathToPose; import frc.robot.command.PointAtPoint; -import frc.robot.command.ManualClimb; import frc.robot.command.PointAtTag; import frc.robot.command.SetPointClimb; import frc.robot.command.Sing; From 0739f1dfe791bbf449466fca243db9a5be7c8b67 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Thu, 14 Mar 2024 09:14:40 -0400 Subject: [PATCH 06/38] [#422] - fixed import error --- src/main/java/frc/robot/subsystems/Climber.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index ce4031e2..62fc4c17 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -13,7 +13,6 @@ import edu.wpi.first.util.datalog.DoubleLogEntry; import frc.robot.Constants; import frc.robot.Constants.ClimbConstants; -import frc.robot.Constants.ClimbConstants.CLIMBER_STATES; import frc.robot.Constants.RobotMap.CAN; import frc.thunder.hardware.ThunderBird; import frc.thunder.shuffleboard.LightningShuffleboard; From d227607a7fff909d8a86f415c5b847f235436d42 Mon Sep 17 00:00:00 2001 From: vilok1 Date: Sun, 17 Mar 2024 07:36:31 -0400 Subject: [PATCH 07/38] [#422] add alliances --- src/main/java/frc/robot/Constants.java | 11 +- src/main/java/frc/robot/RobotContainer.java | 3 + .../java/frc/robot/command/ClimbAlign.java | 108 ++++++++++++++++++ .../java/frc/robot/command/SmartClimb.java | 48 -------- 4 files changed, 119 insertions(+), 51 deletions(-) create mode 100644 src/main/java/frc/robot/command/ClimbAlign.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f1c6de6a..ad57e3ca 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -627,10 +627,15 @@ public class ClimbConstants { // TODO: find real values public static final double CLIMB_SYSTEST_POWER = 0.1; - public static final Pose2d PATHFIND_CENTER_STAGE_START_POSE = new Pose2d(7.43, 4.16, new Rotation2d(Math.PI)); - public static final Pose2d PATHFIND_HIGH_STAGE_START_POSE = new Pose2d(3.55, 6.16, new Rotation2d(Units.degreesToRadians(-53.13))); - public static final Pose2d PATHFIND_LOW_STAGE_START_POSE = new Pose2d(3.54, 1.91, new Rotation2d(Units.degreesToRadians(63.43))); + public static final Pose2d PATHFIND_CENTER_STAGE_START_POSE_BLUE = new Pose2d(7.43, 4.16, new Rotation2d(0)); + public static final Pose2d PATHFIND_HIGH_STAGE_START_POSE_BLUE = new Pose2d(3.55, 6.16, new Rotation2d(Units.degreesToRadians(-53.13))); + public static final Pose2d PATHFIND_LOW_STAGE_START_POSE_BLUE = new Pose2d(3.54, 1.91, new Rotation2d(Units.degreesToRadians(63.43))); + + public static final Pose2d PATHFIND_CENTER_STAGE_START_POSE_RED = new Pose2d(9.07, 4.16, new Rotation2d(0)); + public static final Pose2d PATHFIND_HIGH_STAGE_START_POSE_RED = new Pose2d(12.96, 6.16, new Rotation2d(Units.degreesToRadians(-53.13))); + public static final Pose2d PATHFIND_LOW_STAGE_START_POSE_RED = new Pose2d(12.95, 1.91, new Rotation2d(Units.degreesToRadians(63.43))); } + public class LEDsConstants { public static final int LED_LENGTH = 31; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 03f7ff65..d4be32bb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.Constants.TunerConstants; import frc.robot.Constants.VisionConstants; import frc.robot.command.ChasePieces; +import frc.robot.command.ClimbAlign; import frc.robot.command.Collect; import frc.robot.command.CollectAndGo; import frc.robot.command.CollisionDetection; @@ -212,6 +213,8 @@ protected void configureButtonBindings() { new Trigger(() -> driver.getPOV() == 0).toggleOnTrue(leds.enableState(LED_STATES.DISABLED)); + new Trigger(driver::getBButton).whileTrue(new ClimbAlign(drivetrain)); + /* copilot */ new Trigger(coPilot::getBButton) .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) diff --git a/src/main/java/frc/robot/command/ClimbAlign.java b/src/main/java/frc/robot/command/ClimbAlign.java new file mode 100644 index 00000000..29836d14 --- /dev/null +++ b/src/main/java/frc/robot/command/ClimbAlign.java @@ -0,0 +1,108 @@ +// 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 com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.ClimbConstants; +import frc.robot.subsystems.Swerve; + +public class ClimbAlign extends Command { + private Swerve drivetrain; + private PathFindToAuton pathFind; + + /** Creates a new ClimbAlign. */ + public ClimbAlign(Swerve drivetrain) { + // Use addRequirements() here to declare subsystem dependencies. + this.drivetrain = drivetrain; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + initCommand().schedule(); + } + + // 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) { + pathFind.cancel(); + drivetrain.brake(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return pathFind.isFinished(); + } + + /** + * @return start poses of climb autons + *
  • 0 - center + *
  • 1 - high + *
  • 2 - low + */ + private Pose2d[] getPoses(){ + if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + return new Pose2d[] { + ClimbConstants.PATHFIND_CENTER_STAGE_START_POSE_BLUE, + ClimbConstants.PATHFIND_HIGH_STAGE_START_POSE_BLUE, + ClimbConstants.PATHFIND_LOW_STAGE_START_POSE_BLUE + }; + } + return new Pose2d[] { + ClimbConstants.PATHFIND_CENTER_STAGE_START_POSE_RED, + ClimbConstants.PATHFIND_HIGH_STAGE_START_POSE_RED, + ClimbConstants.PATHFIND_LOW_STAGE_START_POSE_RED + }; + } + + /** + * Determine get differences between robotPose and auton startposes + * @return differences + *
  • 0 - diffCenter + *
  • 1 - diffHigh + *
  • 2 - diffLow + */ + private double[] getDiffs() { + + double diffCenterX = Math.abs(drivetrain.getPose().getTranslation().getX() - getPoses()[0].getTranslation().getX()); + double diffCenterY = Math.abs(drivetrain.getPose().getTranslation().getY() - getPoses()[0].getTranslation().getY()); + double diffCenter = Math.hypot(diffCenterX, diffCenterY); + + double diffHighX = Math.abs(drivetrain.getPose().getTranslation().getX() - getPoses()[1].getTranslation().getX()); + double diffHighY = Math.abs(drivetrain.getPose().getTranslation().getY() - getPoses()[1].getTranslation().getY()); + double diffHigh = Math.hypot(diffHighX, diffHighY); + + double diffLowX = Math.abs(drivetrain.getPose().getTranslation().getX() - getPoses()[2].getTranslation().getX()); + double diffLowY = Math.abs(drivetrain.getPose().getTranslation().getY() - getPoses()[2].getTranslation().getY()); + double diffLow = Math.hypot(diffLowX, diffLowY); + + return new double[] {diffCenter, diffHigh, diffLow}; + } + + /** + * Create the pathfind command based on the shortest distance + * @return command + */ + public Command initCommand(){ + if (getDiffs()[0] < getDiffs()[1] && getDiffs()[0] < getDiffs()[2]) { + pathFind = new PathFindToAuton(PathPlannerPath.fromPathFile("PathFind-CENTER-STAGE"), drivetrain); + } else + if (getDiffs()[1] < getDiffs()[0] && getDiffs()[1] < getDiffs()[2]) { + pathFind = new PathFindToAuton(PathPlannerPath.fromPathFile("PathFind-HIGH-STAGE"), drivetrain); + } else { + pathFind = new PathFindToAuton(PathPlannerPath.fromPathFile("PathFind-LOW-STAGE"), drivetrain); + } + return pathFind; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/command/SmartClimb.java b/src/main/java/frc/robot/command/SmartClimb.java index 101a4ec6..93d43e4e 100644 --- a/src/main/java/frc/robot/command/SmartClimb.java +++ b/src/main/java/frc/robot/command/SmartClimb.java @@ -2,12 +2,10 @@ import java.util.function.DoubleSupplier; -import com.pathplanner.lib.path.PathPlannerPath; import java.util.function.BooleanSupplier; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.ClimbConstants; import frc.robot.Constants.LEDsConstants.LED_STATES; import frc.robot.subsystems.Climber; import frc.robot.subsystems.LEDs; @@ -17,7 +15,6 @@ public class SmartClimb extends Command { private Climber climber; private Swerve drivetrain; - private PathFindToAuton pathFind; private LEDs leds; private DoubleSupplier leftPower; @@ -26,7 +23,6 @@ public class SmartClimb extends Command { private boolean buttonState; private boolean autoClimbEngaged; - private boolean readyToLineUp; /** * SmartClimb to control the climber using the B button and sticks @@ -56,7 +52,6 @@ public void execute() { // Engage Manual Climb whenever sticks are active climber.setPower(leftPower.getAsDouble(), rightPower.getAsDouble()); autoClimbEngaged = false; - readyToLineUp = true; } else if ((buttonState && !bButton.getAsBoolean()) || (drivetrain.isTipped() && !autoClimbEngaged)) { // Auto retract on the falling edge of the B button or if the robot is tipped climber.retract(); @@ -66,9 +61,6 @@ public void execute() { } } else if (bButton.getAsBoolean() && !autoClimbEngaged) { // line up if not currently lining up - if (readyToLineUp) { - lineUp(); - } // Auto deploy climb when B button is pressed and auto climb has not been engaged climber.deploy(); buttonState = true; @@ -85,44 +77,4 @@ public void end(boolean interrupted) { public boolean isFinished() { return false; } - - /** - * Line up the robot to climb - */ - public void lineUp() { - readyToLineUp = false; - - // Determine which path to take based on the robot's current position - double diffCenterX = Math.abs(drivetrain.getPose().getTranslation().getX() - - ClimbConstants.PATHFIND_CENTER_STAGE_START_POSE.getTranslation().getX()); - double diffCenterY = Math.abs(drivetrain.getPose().getTranslation().getY() - - ClimbConstants.PATHFIND_CENTER_STAGE_START_POSE.getTranslation().getY()); - double diffCenter = Math.hypot(diffCenterX, diffCenterY); - - double diffHighX = Math.abs(drivetrain.getPose().getTranslation().getX() - - ClimbConstants.PATHFIND_HIGH_STAGE_START_POSE.getTranslation().getX()); - double diffHighY = Math.abs(drivetrain.getPose().getTranslation().getY() - - ClimbConstants.PATHFIND_HIGH_STAGE_START_POSE.getTranslation().getY()); - double diffHigh = Math.hypot(diffHighX, diffHighY); - - double diffLowX = Math.abs(drivetrain.getPose().getTranslation().getX() - - ClimbConstants.PATHFIND_LOW_STAGE_START_POSE.getTranslation().getX()); - double diffLowY = Math.abs(drivetrain.getPose().getTranslation().getY() - - ClimbConstants.PATHFIND_LOW_STAGE_START_POSE.getTranslation().getY()); - double diffLow = Math.hypot(diffLowX, diffLowY); - - // Create the pathfind command based on the shortest distance - if (diffCenter < diffHigh && diffCenter < diffLow) { - pathFind = new PathFindToAuton( - PathPlannerPath.fromPathFile("Pathfind-CENTER-STAGE"), drivetrain); - } else if (diffHigh < diffCenter && diffHigh < diffLow) { - pathFind = new PathFindToAuton( - PathPlannerPath.fromPathFile("Pathfind-HIGH-STAGE"), drivetrain); - } else { - pathFind = new PathFindToAuton( - PathPlannerPath.fromPathFile("Pathfind-LOW-STAGE"), drivetrain); - } - - pathFind.schedule(); - } } From f73965ef6e5635fe593dc2fc6eac2bc6801c04e4 Mon Sep 17 00:00:00 2001 From: vilok1 Date: Wed, 20 Mar 2024 21:05:58 -0400 Subject: [PATCH 08/38] [#422] fix direction --- src/main/deploy/pathplanner/paths/PathFind-AMP.path | 2 +- .../deploy/pathplanner/paths/PathFind-CENTER-STAGE.path | 6 +++--- src/main/deploy/pathplanner/paths/PathFind-HIGH-STAGE.path | 6 +++--- src/main/deploy/pathplanner/paths/PathFind-LOW-STAGE.path | 6 +++--- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/PathFind-AMP.path b/src/main/deploy/pathplanner/paths/PathFind-AMP.path index ab14b5ba..902b1c46 100644 --- a/src/main/deploy/pathplanner/paths/PathFind-AMP.path +++ b/src/main/deploy/pathplanner/paths/PathFind-AMP.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Top side paths", + "folder": "Align Paths", "previewStartingState": { "rotation": -88.92917554521306, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/PathFind-CENTER-STAGE.path b/src/main/deploy/pathplanner/paths/PathFind-CENTER-STAGE.path index 2d114f11..c608b60f 100644 --- a/src/main/deploy/pathplanner/paths/PathFind-CENTER-STAGE.path +++ b/src/main/deploy/pathplanner/paths/PathFind-CENTER-STAGE.path @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0, + "rotation": -0.2672165304222104, "rotateFast": false }, "reversed": false, - "folder": "Top side paths", + "folder": "Align Paths", "previewStartingState": { - "rotation": 179.41134538376076, + "rotation": 0.42558183706401953, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/PathFind-HIGH-STAGE.path b/src/main/deploy/pathplanner/paths/PathFind-HIGH-STAGE.path index 06cc7440..37252bff 100644 --- a/src/main/deploy/pathplanner/paths/PathFind-HIGH-STAGE.path +++ b/src/main/deploy/pathplanner/paths/PathFind-HIGH-STAGE.path @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -58.495733280795754, + "rotation": 118.59638768795911, "rotateFast": false }, "reversed": false, - "folder": "Top side paths", + "folder": "Align Paths", "previewStartingState": { - "rotation": -52.51998896228982, + "rotation": 119.78139010583, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/PathFind-LOW-STAGE.path b/src/main/deploy/pathplanner/paths/PathFind-LOW-STAGE.path index c4460294..821bba0c 100644 --- a/src/main/deploy/pathplanner/paths/PathFind-LOW-STAGE.path +++ b/src/main/deploy/pathplanner/paths/PathFind-LOW-STAGE.path @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 60.75117366345304, + "rotation": -123.0238158865408, "rotateFast": false }, "reversed": false, - "folder": "Top side paths", + "folder": "Align Paths", "previewStartingState": { - "rotation": 65.6954507340633, + "rotation": -117.74057745665498, "velocity": 0 }, "useDefaultConstraints": true From 3c7ec979f17ecf9e730420b3cc51bac1cbbc0c03 Mon Sep 17 00:00:00 2001 From: vilok1 Date: Wed, 20 Mar 2024 21:32:26 -0400 Subject: [PATCH 09/38] [#422] fix diffs with main --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/command/ManualClimb.java | 42 +++++++++++++++++++ 2 files changed, 43 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/command/ManualClimb.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 14cf3cb1..1367a221 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,12 +28,12 @@ import frc.robot.Constants.TunerConstants; import frc.robot.Constants.VisionConstants; import frc.robot.command.ChasePieces; -import frc.robot.command.ClimbAlign; import frc.robot.command.Collect; import frc.robot.command.CollectAndGo; import frc.robot.command.CollisionDetection; import frc.robot.command.HasPieceAuto; import frc.robot.command.Index; +import frc.robot.command.ManualClimb; import frc.robot.command.MoveToPose; import frc.robot.command.PathFindToAuton; import frc.robot.command.PathToPose; @@ -228,8 +228,6 @@ protected void configureButtonBindings() { new Trigger(() -> driver.getPOV() == 0).toggleOnTrue(leds.enableState(LED_STATES.DISABLED)); - new Trigger(driver::getBButton).whileTrue(new ClimbAlign(drivetrain)); - /* copilot */ new Trigger(coPilot::getBButton) .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) diff --git a/src/main/java/frc/robot/command/ManualClimb.java b/src/main/java/frc/robot/command/ManualClimb.java new file mode 100644 index 00000000..1e47143e --- /dev/null +++ b/src/main/java/frc/robot/command/ManualClimb.java @@ -0,0 +1,42 @@ +package frc.robot.command; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Climber; + +public class ManualClimb extends Command { + + private Climber climber; + private DoubleSupplier leftPower; + private DoubleSupplier rightPower; + + /** + * Creates a new ManualClimb. + * @param leftPower supplier for left motor power + * @param rightPower supplier for right motor power + * @param climber subsystem + */ + public ManualClimb(DoubleSupplier leftPower, DoubleSupplier rightPower, Climber climber) { + this.climber = climber; + this.leftPower = leftPower; + this.rightPower = rightPower; + + addRequirements(climber); + } + + @Override + public void execute() { + climber.setPower(leftPower.getAsDouble(), rightPower.getAsDouble()); + } + + @Override + public void end(boolean interrupted) { + climber.stop(); + } + + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file From ca02fc50823c76f3a85ec8b3b9fa6f82ee74a818 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Thu, 21 Mar 2024 07:20:21 -0400 Subject: [PATCH 10/38] [#473] - added some debugging code --- .../java/frc/robot/command/PointAtPoint.java | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/command/PointAtPoint.java b/src/main/java/frc/robot/command/PointAtPoint.java index e54b6326..14fdc713 100644 --- a/src/main/java/frc/robot/command/PointAtPoint.java +++ b/src/main/java/frc/robot/command/PointAtPoint.java @@ -18,7 +18,7 @@ public class PointAtPoint extends Command { - private static final double MIN_POWER = 0.3; + private double minPower = 0.3; private Swerve drivetrain; private XboxController driver; @@ -104,6 +104,16 @@ public void initLogging() { inToleranceLog = new BooleanLogEntry(log, "/PointAtPoint/InTolerance"); } + + public void setDebugging(){ + + headingController.setP(LightningShuffleboard.getDouble("Point-At-Point", "P", headingController.getP())); + headingController.setI(LightningShuffleboard.getDouble("Point-At-Point", "I", headingController.getI())); + headingController.setD(LightningShuffleboard.getDouble("Point-At-Point", "D", headingController.getD())); + minPower = LightningShuffleboard.getDouble("Point-At-Point", "Min Power", minPower); + + } + @Override public void execute() { Pose2d pose = drivetrain.getPose(); @@ -114,8 +124,10 @@ public void execute() { targetHeading %= 360; pidOutput = headingController.calculate((pose.getRotation().getDegrees() + 360) % 360, targetHeading); - if (!inTolerance() && Math.abs(pidOutput) < MIN_POWER) { - pidOutput = Math.signum(pidOutput) * MIN_POWER; + setDebugging(); + + if (!inTolerance() && Math.abs(pidOutput) < minPower) { + pidOutput = Math.signum(pidOutput) * minPower; } drivetrain.setField(-driver.getLeftY(), -driver.getLeftX(), pidOutput); From 0c6e1637558c4aa23a6b0bcd49b8d8e1c8e15743 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Thu, 21 Mar 2024 19:11:23 -0400 Subject: [PATCH 11/38] [#473] - reformatted tuning code --- src/main/java/frc/robot/command/PointAtPoint.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/command/PointAtPoint.java b/src/main/java/frc/robot/command/PointAtPoint.java index 14fdc713..79a9d347 100644 --- a/src/main/java/frc/robot/command/PointAtPoint.java +++ b/src/main/java/frc/robot/command/PointAtPoint.java @@ -124,7 +124,9 @@ public void execute() { targetHeading %= 360; pidOutput = headingController.calculate((pose.getRotation().getDegrees() + 360) % 360, targetHeading); - setDebugging(); + if (!DriverStation.isFMSAttached()){ + setDebugging(); + } if (!inTolerance() && Math.abs(pidOutput) < minPower) { pidOutput = Math.signum(pidOutput) * minPower; From 25e0be2cc70848d31da17d1cc6d6309184fd66ba Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Thu, 21 Mar 2024 20:22:46 -0400 Subject: [PATCH 12/38] [#473] - commented climb, retuned pid --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 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 30f50263..cd2b8d75 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -323,7 +323,7 @@ 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 PIDController TAG_AIM_CONTROLLER = new PIDController(0.1, 0, 0, 0.01); + public static final PIDController TAG_AIM_CONTROLLER = new PIDController(0.2, 0, 0, 0.01); 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; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 455eb6c4..b35468c1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -87,7 +87,7 @@ public class RobotContainer extends LightningContainer { private Flywheel flywheel; private Pivot pivot; private Indexer indexer; - private Climber climber; +// private Climber climber; LEDs leds; Orchestra sing; @@ -128,7 +128,7 @@ protected void initializeSubsystems() { flywheel = new Flywheel(); pivot = Constants.isMercury() ? new PivotMercury() : new PivotRhapsody(); indexer = new Indexer(collector); - climber = new Climber(); + // climber = new Climber(); leds = new LEDs(); sing = new Orchestra(); @@ -350,7 +350,7 @@ protected void configureDefaultCommands() { // () -> -coPilot.getRightY(), // coPilot::getYButton).deadlineWith(leds.enableState(LED_STATES.CLIMBING))); - climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber)); + // climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber)); } protected Command getAutonomousCommand() { From 92858d0da88709220ec2006b21ae05aafbae7218 Mon Sep 17 00:00:00 2001 From: Driver Station <126530662+driverstation862@users.noreply.github.com> Date: Thu, 21 Mar 2024 21:09:25 -0400 Subject: [PATCH 13/38] [#466] disabled flywheel in auton --- .../java/frc/robot/command/SmartCollect.java | 23 +++++++++++++++---- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/command/SmartCollect.java b/src/main/java/frc/robot/command/SmartCollect.java index 0831f5c9..89036edf 100644 --- a/src/main/java/frc/robot/command/SmartCollect.java +++ b/src/main/java/frc/robot/command/SmartCollect.java @@ -2,6 +2,7 @@ import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.Constants.IndexerConstants.PieceState; @@ -48,7 +49,11 @@ public SmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, this.collectorPower = collectorPower; this.indexerPower = indexerPower; - addRequirements(collector, indexer, flywheel); + if (DriverStation.isAutonomous()) { + addRequirements(collector, indexer); + } else { + addRequirements(collector, indexer, flywheel); + } } @Override @@ -68,7 +73,9 @@ public void execute() { collector.setPower(collectorPower.getAsDouble()); if (allowIndex) { indexer.setPower(indexerPower.getAsDouble()); - flywheel.setAllMotorsRPM(-200); + if (!DriverStation.isAutonomous()) { + flywheel.setAllMotorsRPM(-200); + } } break; @@ -77,7 +84,9 @@ public void execute() { // Slow down collector to prevent jamming collector.setPower(0.65 * collectorPower.getAsDouble()); indexer.setPower(indexerPower.getAsDouble()); - flywheel.setAllMotorsRPM(-500); + if(!DriverStation.isAutonomous()){ + flywheel.setAllMotorsRPM(-500); + } } else { // Stop collecting since pivot is not in right place collector.stop(); @@ -91,7 +100,9 @@ public void execute() { indexer.setPower(0.9 * indexerPower.getAsDouble()); } else if (reversedFromExit) { indexer.stop(); - flywheel.coast(true); + if(!DriverStation.isAutonomous()){ + flywheel.coast(true); + } new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); } break; @@ -108,7 +119,9 @@ public void execute() { public void end(boolean interrupted) { collector.stop(); indexer.stop(); - flywheel.coast(true); + if(!DriverStation.isAutonomous()){ + flywheel.coast(true); + } System.out.println("COLLECT - Smart Collect END"); } From 45869dac73243e220013993e00e6bb20e4c96f39 Mon Sep 17 00:00:00 2001 From: Driver Station <126530662+driverstation862@users.noreply.github.com> Date: Thu, 21 Mar 2024 21:32:59 -0400 Subject: [PATCH 14/38] [#466] ended collector and indexer once note passes indexer beam break --- .../frc/robot/command/AutonSmartCollect.java | 133 ++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 src/main/java/frc/robot/command/AutonSmartCollect.java diff --git a/src/main/java/frc/robot/command/AutonSmartCollect.java b/src/main/java/frc/robot/command/AutonSmartCollect.java new file mode 100644 index 00000000..ea10f36f --- /dev/null +++ b/src/main/java/frc/robot/command/AutonSmartCollect.java @@ -0,0 +1,133 @@ +package frc.robot.command; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.Constants.IndexerConstants.PieceState; +import frc.robot.subsystems.Collector; +import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Indexer; +import frc.robot.subsystems.Pivot; +import frc.thunder.command.TimedCommand; + +public class AutonSmartCollect extends Command { + + private DoubleSupplier collectorPower; + private DoubleSupplier indexerPower; + private Collector collector; + private Indexer indexer; + private Pivot pivot; + private Flywheel flywheel; + + /* Used to prevent indexing if pivot angle is too high */ + private boolean allowIndex; + + /* + * Used to check if we have already touched the exit beambreak and we need to + * back down + */ + private boolean reversedFromExit = false; + + /** + * SmartCollect to control collector and indexer using beambreaks + * + * @param collectorPower power to apply to collector + * @param indexerPower power to apply to indexer + * @param collector subsystem + * @param indexer subsystem + * @param pivot subsystem (read only) + * @param flywheel subsystem + */ + public AutonSmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, Collector collector, + Indexer indexer, Pivot pivot, Flywheel flywheel) { + this.collector = collector; + this.indexer = indexer; + this.pivot = pivot; + this.flywheel = flywheel; + this.collectorPower = collectorPower; + this.indexerPower = indexerPower; + + if (DriverStation.isAutonomous()) { + addRequirements(collector, indexer); + } else { + addRequirements(collector, indexer, flywheel); + } + } + + @Override + public void initialize() { + reversedFromExit = false; + + System.out.println("COLLECT - Smart Collect INIT"); + } + + @Override + public void execute() { + allowIndex = pivot.getAngle() < pivot.getMaxIndexAngle(); + + switch (indexer.getPieceState()) { + case NONE: /* Note not passing any beambreaks */ + reversedFromExit = false; + collector.setPower(collectorPower.getAsDouble()); + if (allowIndex) { + indexer.setPower(indexerPower.getAsDouble()); + if (!DriverStation.isAutonomous()) { + flywheel.setAllMotorsRPM(-200); + } + } + break; + + case IN_COLLECT: /* Note has passed beambreak past collector */ + if (allowIndex) { + // Slow down collector to prevent jamming + collector.setPower(0.65 * collectorPower.getAsDouble()); + indexer.setPower(indexerPower.getAsDouble()); + if(!DriverStation.isAutonomous()){ + flywheel.setAllMotorsRPM(-500); + } + } else { + // Stop collecting since pivot is not in right place + collector.stop(); + indexer.stop(); + } + break; + + case IN_PIVOT: /* Note has touched entry indexer beambreak */ + collector.stop(); + if (allowIndex && !reversedFromExit) { + indexer.setPower(0.9 * indexerPower.getAsDouble()); + } else if (reversedFromExit) { + indexer.stop(); + if(!DriverStation.isAutonomous()){ + flywheel.coast(true); + } + new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); + } + break; + + case IN_INDEXER: /* Note has touched exit indexer beambreak */ + end(false); + indexer.setPower(-0.25 * indexerPower.getAsDouble()); + reversedFromExit = true; + break; + } + } + + @Override + public void end(boolean interrupted) { + collector.stop(); + indexer.stop(); + if(!DriverStation.isAutonomous()){ + flywheel.coast(true); + } + + System.out.println("COLLECT - Smart Collect END"); + } + + @Override + public boolean isFinished() { + return reversedFromExit && indexer.getPieceState() == PieceState.IN_PIVOT; + } +} From 96eff612c34600978d5325588516002e7407b738 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Thu, 21 Mar 2024 21:36:10 -0400 Subject: [PATCH 15/38] [#473] - finalized --- src/main/java/frc/robot/command/PointAtPoint.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/command/PointAtPoint.java b/src/main/java/frc/robot/command/PointAtPoint.java index 79a9d347..3b8a36c2 100644 --- a/src/main/java/frc/robot/command/PointAtPoint.java +++ b/src/main/java/frc/robot/command/PointAtPoint.java @@ -132,11 +132,13 @@ public void execute() { pidOutput = Math.signum(pidOutput) * minPower; } + drivetrain.setField(-driver.getLeftY(), -driver.getLeftX(), pidOutput); LightningShuffleboard.setDouble("Point-At-Point", "Target Heading", targetHeading); LightningShuffleboard.setDouble("Point-At-Point", "Current Heading", drivetrain.getPose().getRotation().getDegrees()); + LightningShuffleboard.setBool("Point-At-Point", "In Tolerance", inTolerance()); updateLogging(); } From aa2cc468e6399c4c32be74681492c476b4f3381a Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 22 Mar 2024 07:31:01 -0400 Subject: [PATCH 16/38] [#476] Encoder Offset --- src/main/java/frc/robot/Constants.java | 5 ++--- src/main/java/frc/robot/subsystems/Climber.java | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 30f50263..6234cc7c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -517,13 +517,12 @@ public class RhapsodyPivotConstants { // TODO: get real public static final double ANGLE_TOLERANCE = 0.00208d; - public static final double ENCODER_OFFSET = -0.54008; // In rotations, was 0.282 //TODO: get real :) + public static final double ENCODER_OFFSET = -0.913834;//-0.54008; // In rotations public static final SensorDirectionValue ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; public static final double ENCODER_TO_MECHANISM_RATIO = 1d; public static final double ROTOR_TO_ENCODER_RATIO = 275d; - public static final double BIAS_INCREMENT = 1d; // fDegrees to bias by per button press TODO get amount to bias - // by + public static final double BIAS_INCREMENT = 1d; // fDegrees to bias by per button press TODO get amount to bias by public static final double STOW_ANGLE = 27d; diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 6baba0ed..3ed9e5b6 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -74,7 +74,7 @@ public Climber() { climbMotorL.setPosition(0d); climbMotorR.setPosition(0d); - // initOldLogging(); + initOldLogging(); initLogging(); } From 48376a7d82fbac4cdaae9573b783171127b811af Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 22 Mar 2024 18:57:27 -0400 Subject: [PATCH 17/38] [#472] Retuned and made spot for tuning --- src/main/java/frc/robot/Constants.java | 56 ++++++++++++++----- .../frc/robot/command/shoot/SmartShoot.java | 11 +++- .../java/frc/robot/command/shoot/preAim.java | 11 +++- 3 files changed, 59 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 30f50263..2155f1f1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -39,8 +39,7 @@ public static final boolean isMercury() { public class DrivetrainConstants { // TODO Get new for new robot public static final double MaxSpeed = 6; // 6 meters per second desired top speed - private static final double WHEELBASE = TunerConstants.kFrontLeftXPosInches * 2; // 2 * x distance from center - // of robot to wheel + private static final double WHEELBASE = TunerConstants.kFrontLeftXPosInches * 2; // 2 * x distance from center of robot to wheel public static final double MaxAngularRate = 2 * Math.PI * ( // convert to radians per second TunerConstants.kSpeedAt12VoltsMps / Math.PI * Math.sqrt(2 * Math.pow(WHEELBASE, 2))); // free speed / circumference of circle with radius of wheelbase @@ -132,8 +131,7 @@ public static class AutonomousConstants { public static final double CONTROL_LOOP_PERIOD = 0.01; - public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); // TODO get - // constants + public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); // TODO get constants public static final double BLUE_CHASE_BOUNDARY = 8.5; // The highest X value the robot can be at before ending. // Prevents going over center line. @@ -426,19 +424,19 @@ public class FlywheelConstants { public static final double TOP_1_MOTOR_KA = 0; // SLOT 0 BOTTOM, 0 - 49 RPS - public static final double BOTTOM_0_MOTOR_KP = 0.15; + public static final double BOTTOM_0_MOTOR_KP = 0.155; public static final double BOTTOM_0_MOTOR_KI = 0; public static final double BOTTOM_0_MOTOR_KD = 0; - public static final double BOTTOM_0_MOTOR_KS = 0.3; - public static final double BOTTOM_0_MOTOR_KV = 0.11; + public static final double BOTTOM_0_MOTOR_KS = 0.35; + public static final double BOTTOM_0_MOTOR_KV = 0.112; public static final double BOTTOM_0_MOTOR_KA = 0; // SLOT 1 BOTTOM, 50 - 107 RPS - public static final double BOTTOM_1_MOTOR_KP = 0.16; + public static final double BOTTOM_1_MOTOR_KP = 0.15; public static final double BOTTOM_1_MOTOR_KI = 0; public static final double BOTTOM_1_MOTOR_KD = 0; - public static final double BOTTOM_1_MOTOR_KS = 0.3; - public static final double BOTTOM_1_MOTOR_KV = 0.1155; + public static final double BOTTOM_1_MOTOR_KS = 0.35; + public static final double BOTTOM_1_MOTOR_KV = 0.114; public static final double BOTTOM_1_MOTOR_KA = 0; public static final double RPM_TOLERANCE = 50d; @@ -540,7 +538,7 @@ public class ShooterConstants { public static final double FAR_WING_X = 3.3; // Distance in meters, angle in degrees - public static final InterpolationMap ANGLE_MAP = new InterpolationMap() { + public static final InterpolationMap TUBE_ANGLE_MAP = new InterpolationMap() { { // As distance gets smaller angle goes up put(1.21d, 52d); @@ -554,7 +552,35 @@ public class ShooterConstants { }; // Distance in meters, speed in RPM - public static final InterpolationMap SPEED_MAP = new InterpolationMap() { + public static final InterpolationMap TUBE_SPEED_MAP = new InterpolationMap() { + { + // As distance get smaller RPM gets smaller + put(1.21d, 2000d); + put(2d, 2500d); + put(2.5d, 3000d); + put(3d, 3500d); + put(3.5d, 4000d); + put(4.09d, 3600d); + put(4.86d, 4600d); + } + }; + + // Distance in meters, angle in degrees + public static final InterpolationMap STEALTH_ANGLE_MAP = new InterpolationMap() { + { + // As distance gets smaller angle goes up + put(1.21d, 52d); + put(2d, 45d); + put(2.5d, 41.5d); + put(3d, 37d); + put(3.5d, 30d); + put(4.09d, 26.5d); + put(4.86, 26.5d); + } + }; + + // Distance in meters, speed in RPM + public static final InterpolationMap STEALTH_SPEED_MAP = new InterpolationMap() { { // As distance get smaller RPM gets smaller put(1.21d, 2000d); @@ -655,9 +681,9 @@ public class LEDsConstants { public static final Map STRAND_LENGTH = new HashMap() { { - put(-1, LEDsConstants.LED_LENGTH); - put(0, 14); - put(1, 14); + put(-1, LEDsConstants.LED_LENGTH); + put(0, 14); + put(1, 14); } }; diff --git a/src/main/java/frc/robot/command/shoot/SmartShoot.java b/src/main/java/frc/robot/command/shoot/SmartShoot.java index 88c56d12..c07833e8 100644 --- a/src/main/java/frc/robot/command/shoot/SmartShoot.java +++ b/src/main/java/frc/robot/command/shoot/SmartShoot.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; +import frc.robot.Constants; import frc.robot.RobotContainer; import frc.robot.Constants.CandConstants; import frc.robot.Constants.LEDsConstants.LED_STATES; @@ -144,7 +145,10 @@ public boolean onTarget() { * @return Angle to set pivot to */ public double calculateTargetAngle(double distance) { - return ShooterConstants.ANGLE_MAP.get(distance); + if(Constants.isMercury()){ + return ShooterConstants.TUBE_ANGLE_MAP.get(distance); + } + return ShooterConstants.STEALTH_ANGLE_MAP.get(distance); } /** @@ -153,6 +157,9 @@ public double calculateTargetAngle(double distance) { * @return RPM to set the Flywheels */ public double calculateTargetRPM(double distance) { - return ShooterConstants.SPEED_MAP.get(distance); + if(Constants.isMercury()){ + return ShooterConstants.TUBE_SPEED_MAP.get(distance); + } + return ShooterConstants.STEALTH_SPEED_MAP.get(distance); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/command/shoot/preAim.java b/src/main/java/frc/robot/command/shoot/preAim.java index 49f64f6c..f3025328 100644 --- a/src/main/java/frc/robot/command/shoot/preAim.java +++ b/src/main/java/frc/robot/command/shoot/preAim.java @@ -1,6 +1,7 @@ package frc.robot.command.shoot; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Pivot; @@ -49,7 +50,10 @@ public boolean isFinished() { * @return Angle to set pivot to */ public double calculateTargetAngle(double distance) { - return ShooterConstants.ANGLE_MAP.get(distance); + if(Constants.isMercury()){ + return ShooterConstants.TUBE_ANGLE_MAP.get(distance); + } + return ShooterConstants.STEALTH_ANGLE_MAP.get(distance); } /** @@ -59,6 +63,9 @@ public double calculateTargetAngle(double distance) { * @return RPM to set the Flywheels */ public double calculateTargetRPM(double distance) { - return ShooterConstants.SPEED_MAP.get(distance); + if(Constants.isMercury()){ + return ShooterConstants.TUBE_SPEED_MAP.get(distance); + } + return ShooterConstants.STEALTH_SPEED_MAP.get(distance); } } From ba4f00e77e68cd8e545c0b6644e9f41266b561d0 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Fri, 22 Mar 2024 19:03:23 -0400 Subject: [PATCH 18/38] [#466] reformating (needs testing!) --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../frc/robot/command/AutonSmartCollect.java | 31 ++++--------------- 2 files changed, 8 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 455eb6c4..32d14e35 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc.robot.Constants.PathFindingConstants; import frc.robot.Constants.LEDsConstants.LED_STATES; import frc.robot.Constants.TunerConstants; +import frc.robot.command.AutonSmartCollect; import frc.robot.command.ChasePieces; import frc.robot.command.Collect; import frc.robot.command.CollectAndGo; @@ -165,7 +166,7 @@ protected void initializeNamedCommands() { NamedCommands.registerCommand("Chase-Pieces", new ChasePieces(drivetrain, collector, indexer, pivot, flywheel, limelights)); NamedCommands.registerCommand("Smart-Collect", - new SmartCollect(() -> .5d, () -> .6d, collector, indexer, pivot, flywheel) + new AutonSmartCollect(() -> 0.5, () -> 0.6, collector, indexer, pivot) .deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1))); NamedCommands.registerCommand("Index-Up", new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); NamedCommands.registerCommand("PathFind", new PathToPose(PathFindingConstants.TEST_POSE)); diff --git a/src/main/java/frc/robot/command/AutonSmartCollect.java b/src/main/java/frc/robot/command/AutonSmartCollect.java index ea10f36f..0ebc83e4 100644 --- a/src/main/java/frc/robot/command/AutonSmartCollect.java +++ b/src/main/java/frc/robot/command/AutonSmartCollect.java @@ -7,7 +7,6 @@ import frc.robot.RobotContainer; import frc.robot.Constants.IndexerConstants.PieceState; import frc.robot.subsystems.Collector; -import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; import frc.robot.subsystems.Pivot; import frc.thunder.command.TimedCommand; @@ -19,7 +18,6 @@ public class AutonSmartCollect extends Command { private Collector collector; private Indexer indexer; private Pivot pivot; - private Flywheel flywheel; /* Used to prevent indexing if pivot angle is too high */ private boolean allowIndex; @@ -38,22 +36,17 @@ public class AutonSmartCollect extends Command { * @param collector subsystem * @param indexer subsystem * @param pivot subsystem (read only) - * @param flywheel subsystem */ public AutonSmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, Collector collector, - Indexer indexer, Pivot pivot, Flywheel flywheel) { + Indexer indexer, Pivot pivot) { + this.collectorPower = collectorPower; + this.indexerPower = indexerPower; + this.collector = collector; this.indexer = indexer; this.pivot = pivot; - this.flywheel = flywheel; - this.collectorPower = collectorPower; - this.indexerPower = indexerPower; - if (DriverStation.isAutonomous()) { - addRequirements(collector, indexer); - } else { - addRequirements(collector, indexer, flywheel); - } + addRequirements(collector, indexer); } @Override @@ -73,9 +66,6 @@ public void execute() { collector.setPower(collectorPower.getAsDouble()); if (allowIndex) { indexer.setPower(indexerPower.getAsDouble()); - if (!DriverStation.isAutonomous()) { - flywheel.setAllMotorsRPM(-200); - } } break; @@ -84,9 +74,6 @@ public void execute() { // Slow down collector to prevent jamming collector.setPower(0.65 * collectorPower.getAsDouble()); indexer.setPower(indexerPower.getAsDouble()); - if(!DriverStation.isAutonomous()){ - flywheel.setAllMotorsRPM(-500); - } } else { // Stop collecting since pivot is not in right place collector.stop(); @@ -100,9 +87,6 @@ public void execute() { indexer.setPower(0.9 * indexerPower.getAsDouble()); } else if (reversedFromExit) { indexer.stop(); - if(!DriverStation.isAutonomous()){ - flywheel.coast(true); - } new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); } break; @@ -119,11 +103,8 @@ public void execute() { public void end(boolean interrupted) { collector.stop(); indexer.stop(); - if(!DriverStation.isAutonomous()){ - flywheel.coast(true); - } - System.out.println("COLLECT - Smart Collect END"); + System.out.println("COLLECT - Auton Smart Collect END"); } @Override From b8ee3365b07dd74b72b39f06da18016b15920097 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 22 Mar 2024 19:42:19 -0400 Subject: [PATCH 19/38] [#473] Cleaned --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- src/main/java/frc/robot/command/PointAtPoint.java | 6 +----- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b35468c1..455eb6c4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -87,7 +87,7 @@ public class RobotContainer extends LightningContainer { private Flywheel flywheel; private Pivot pivot; private Indexer indexer; -// private Climber climber; + private Climber climber; LEDs leds; Orchestra sing; @@ -128,7 +128,7 @@ protected void initializeSubsystems() { flywheel = new Flywheel(); pivot = Constants.isMercury() ? new PivotMercury() : new PivotRhapsody(); indexer = new Indexer(collector); - // climber = new Climber(); + climber = new Climber(); leds = new LEDs(); sing = new Orchestra(); @@ -350,7 +350,7 @@ protected void configureDefaultCommands() { // () -> -coPilot.getRightY(), // coPilot::getYButton).deadlineWith(leds.enableState(LED_STATES.CLIMBING))); - // climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber)); + climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber)); } protected Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/command/PointAtPoint.java b/src/main/java/frc/robot/command/PointAtPoint.java index 3b8a36c2..30fea74f 100644 --- a/src/main/java/frc/robot/command/PointAtPoint.java +++ b/src/main/java/frc/robot/command/PointAtPoint.java @@ -106,12 +106,10 @@ public void initLogging() { public void setDebugging(){ - headingController.setP(LightningShuffleboard.getDouble("Point-At-Point", "P", headingController.getP())); headingController.setI(LightningShuffleboard.getDouble("Point-At-Point", "I", headingController.getI())); headingController.setD(LightningShuffleboard.getDouble("Point-At-Point", "D", headingController.getD())); minPower = LightningShuffleboard.getDouble("Point-At-Point", "Min Power", minPower); - } @Override @@ -124,9 +122,7 @@ public void execute() { targetHeading %= 360; pidOutput = headingController.calculate((pose.getRotation().getDegrees() + 360) % 360, targetHeading); - if (!DriverStation.isFMSAttached()){ - setDebugging(); - } + // setDebugging(); if (!inTolerance() && Math.abs(pidOutput) < minPower) { pidOutput = Math.signum(pidOutput) * minPower; From a62413b62c36fb48bc9d7491e23093dda5898532 Mon Sep 17 00:00:00 2001 From: vilok1 Date: Fri, 22 Mar 2024 19:50:42 -0400 Subject: [PATCH 20/38] [#422] fix linter --- src/main/java/frc/robot/command/ClimbAlign.java | 4 +++- src/main/java/frc/robot/command/PathFindToAuton.java | 6 ------ 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/command/ClimbAlign.java b/src/main/java/frc/robot/command/ClimbAlign.java index 29836d14..6ef6c290 100644 --- a/src/main/java/frc/robot/command/ClimbAlign.java +++ b/src/main/java/frc/robot/command/ClimbAlign.java @@ -16,7 +16,9 @@ public class ClimbAlign extends Command { private Swerve drivetrain; private PathFindToAuton pathFind; - /** Creates a new ClimbAlign. */ + /** Creates a new ClimbAlign. + * @param drivetrain + */ public ClimbAlign(Swerve drivetrain) { // Use addRequirements() here to declare subsystem dependencies. this.drivetrain = drivetrain; diff --git a/src/main/java/frc/robot/command/PathFindToAuton.java b/src/main/java/frc/robot/command/PathFindToAuton.java index 9ccaf1ea..a817c6d5 100644 --- a/src/main/java/frc/robot/command/PathFindToAuton.java +++ b/src/main/java/frc/robot/command/PathFindToAuton.java @@ -10,12 +10,9 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.AutonomousConstants; import frc.robot.subsystems.Swerve; -import frc.thunder.filter.XboxControllerFilter; public class PathFindToAuton extends Command { - private Swerve drivetrain; - private XboxControllerFilter controller; // Driver Controller private AutoBuilder autoBuilder; private Command pathFindCommand; private PathPlannerPath autonPath; @@ -25,11 +22,8 @@ public class PathFindToAuton extends Command { * * @param autonPath * @param drivetrain - * @param controller */ public PathFindToAuton(PathPlannerPath autonPath, Swerve drivetrain) { - this.drivetrain = drivetrain; - this.controller = controller; this.autonPath = autonPath; autoBuilder = new AutoBuilder(); From d76b6f0348259446934a8271f41d9374cdc11384 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Fri, 22 Mar 2024 21:33:18 -0400 Subject: [PATCH 21/38] [#466] tested and works --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/command/AutonSmartCollect.java | 68 ++++--------------- .../java/frc/robot/subsystems/Collector.java | 11 ++- .../java/frc/robot/subsystems/Indexer.java | 9 +++ 5 files changed, 37 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 30f50263..3767c782 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -457,6 +457,8 @@ public enum PieceState { IN_COLLECT, IN_PIVOT, IN_INDEXER, NONE } + + public static final boolean INDEXER_MOTOR_BRAKE_MODE = true; public static final double INDEXER_DEFAULT_POWER = 1d; public static final double INDEXER_MANUAL_POWER = 1d; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 32d14e35..e878e0d5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -166,7 +166,7 @@ protected void initializeNamedCommands() { NamedCommands.registerCommand("Chase-Pieces", new ChasePieces(drivetrain, collector, indexer, pivot, flywheel, limelights)); NamedCommands.registerCommand("Smart-Collect", - new AutonSmartCollect(() -> 0.5, () -> 0.6, collector, indexer, pivot) + new AutonSmartCollect(() -> 0.5, () -> 0.6, collector, indexer) .deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1))); NamedCommands.registerCommand("Index-Up", new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); NamedCommands.registerCommand("PathFind", new PathToPose(PathFindingConstants.TEST_POSE)); @@ -237,6 +237,8 @@ protected void configureButtonBindings() { .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, flywheel)) .deadlineWith(leds.enableState(LED_STATES.COLLECTING))); + + // .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, flywheel)) // cand shots for the robot new Trigger(coPilot::getAButton) diff --git a/src/main/java/frc/robot/command/AutonSmartCollect.java b/src/main/java/frc/robot/command/AutonSmartCollect.java index 0ebc83e4..964f5f9e 100644 --- a/src/main/java/frc/robot/command/AutonSmartCollect.java +++ b/src/main/java/frc/robot/command/AutonSmartCollect.java @@ -8,7 +8,6 @@ import frc.robot.Constants.IndexerConstants.PieceState; import frc.robot.subsystems.Collector; import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Pivot; import frc.thunder.command.TimedCommand; public class AutonSmartCollect extends Command { @@ -17,16 +16,6 @@ public class AutonSmartCollect extends Command { private DoubleSupplier indexerPower; private Collector collector; private Indexer indexer; - private Pivot pivot; - - /* Used to prevent indexing if pivot angle is too high */ - private boolean allowIndex; - - /* - * Used to check if we have already touched the exit beambreak and we need to - * back down - */ - private boolean reversedFromExit = false; /** * SmartCollect to control collector and indexer using beambreaks @@ -38,64 +27,33 @@ public class AutonSmartCollect extends Command { * @param pivot subsystem (read only) */ public AutonSmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, Collector collector, - Indexer indexer, Pivot pivot) { + Indexer indexer) { this.collectorPower = collectorPower; this.indexerPower = indexerPower; - + this.collector = collector; this.indexer = indexer; - this.pivot = pivot; addRequirements(collector, indexer); } @Override public void initialize() { - reversedFromExit = false; + collector.setPower(collectorPower.getAsDouble()); + indexer.setPower(indexerPower.getAsDouble()); - System.out.println("COLLECT - Smart Collect INIT"); + System.out.println("COLLECT - Auton Smart Collect INIT"); } @Override public void execute() { - allowIndex = pivot.getAngle() < pivot.getMaxIndexAngle(); - - switch (indexer.getPieceState()) { - case NONE: /* Note not passing any beambreaks */ - reversedFromExit = false; - collector.setPower(collectorPower.getAsDouble()); - if (allowIndex) { - indexer.setPower(indexerPower.getAsDouble()); - } - break; - - case IN_COLLECT: /* Note has passed beambreak past collector */ - if (allowIndex) { - // Slow down collector to prevent jamming - collector.setPower(0.65 * collectorPower.getAsDouble()); - indexer.setPower(indexerPower.getAsDouble()); - } else { - // Stop collecting since pivot is not in right place - collector.stop(); - indexer.stop(); - } - break; - - case IN_PIVOT: /* Note has touched entry indexer beambreak */ - collector.stop(); - if (allowIndex && !reversedFromExit) { - indexer.setPower(0.9 * indexerPower.getAsDouble()); - } else if (reversedFromExit) { - indexer.stop(); - new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); - } - break; - - case IN_INDEXER: /* Note has touched exit indexer beambreak */ - end(false); - indexer.setPower(-0.25 * indexerPower.getAsDouble()); - reversedFromExit = true; - break; + if(indexer.getPieceState() == PieceState.IN_COLLECT){ + indexer.setPower(indexerPower.getAsDouble() / 2); + } else if (indexer.getPieceState() == PieceState.IN_PIVOT) { + end(false); + } else { + collector.setPower(collectorPower.getAsDouble()); + indexer.setPower(indexerPower.getAsDouble()); } } @@ -109,6 +67,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return reversedFromExit && indexer.getPieceState() == PieceState.IN_PIVOT; + return false; } } diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index 0768b088..61589bef 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -73,7 +73,7 @@ private void initLogging() { */ public boolean getEntryBeamBreakState() { if (Constants.isMercury()) { - return entryDebouncer.calculate(!beamBreak.get()); + return entryDebouncer.calculate(beamBreak.get()); } return entryDebouncer.calculate(beamBreak.get()); } @@ -89,6 +89,15 @@ public void setPower(double power) { motor.setControl(velocityVoltage.withVelocity(power)); } + /** + * gets the current power of the collector motor + * + * @return the current power of the collector motor + */ + public double getPower() { + return motor.get(); + } + @Override public void periodic() { // tells robot if we have a piece in collector diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 0d801204..f1d2c325 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -117,6 +117,15 @@ public void setPower(double power) { indexerMotor.setControl(dutyCycleControl.withOutput(power)); } + /** + * Get the current power of the indexer motor + * + * @return current power of the indexer motor + */ + public double getPower() { + return indexerMotor.get(); + } + /** * Index up */ From 33056bb38979338682d4f33fbc3b408e24fc02a3 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Fri, 22 Mar 2024 21:38:21 -0400 Subject: [PATCH 22/38] [#466] fixed linter issues --- src/main/java/frc/robot/command/AutonSmartCollect.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/command/AutonSmartCollect.java b/src/main/java/frc/robot/command/AutonSmartCollect.java index 964f5f9e..f5a07ea2 100644 --- a/src/main/java/frc/robot/command/AutonSmartCollect.java +++ b/src/main/java/frc/robot/command/AutonSmartCollect.java @@ -24,7 +24,6 @@ public class AutonSmartCollect extends Command { * @param indexerPower power to apply to indexer * @param collector subsystem * @param indexer subsystem - * @param pivot subsystem (read only) */ public AutonSmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, Collector collector, Indexer indexer) { From 66a21f66df49728324d48a34ef6b9c1e3071b21f Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Fri, 22 Mar 2024 21:57:43 -0400 Subject: [PATCH 23/38] [#483] changed (easy) --- .../java/frc/robot/subsystems/Indexer.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index f1d2c325..5c7ae045 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -23,7 +23,7 @@ public class Indexer extends SubsystemBase { private Collector collector; - private ThunderBird indexerMotor; + private ThunderBird motor; private DigitalInput indexerSensorEntry = new DigitalInput(DIO.INDEXER_ENTER_BEAMBREAK); private DigitalInput indexerSensorExit = new DigitalInput(DIO.INDEXER_EXIT_BEAMBREAK); @@ -51,13 +51,13 @@ public class Indexer extends SubsystemBase { public Indexer(Collector collector) { this.collector = collector; - indexerMotor = new ThunderBird(CAN.INDEXER_MOTOR, CAN.CANBUS_FD, + motor = new ThunderBird(CAN.INDEXER_MOTOR, CAN.CANBUS_FD, IndexerConstants.MOTOR_INVERT, IndexerConstants.MOTOR_STATOR_CURRENT_LIMIT, IndexerConstants.INDEXER_MOTOR_BRAKE_MODE); - indexerMotor.configSupplyLimit(0d); - indexerMotor.configStatorLimit(80d); + motor.configSupplyLimit(0d); + motor.configStatorLimit(80d); - indexerMotor.applyConfig(); + motor.applyConfig(); initLogging(); } @@ -77,7 +77,7 @@ private void initLogging() { isExitingLog = new BooleanLogEntry(log, "/Indexer/IsExiting"); hasPieceLog = new BooleanLogEntry(log, "/Indexer/HasPiece"); - LightningShuffleboard.setDoubleSupplier("Indexer", "Power", () -> indexerMotor.get()); + LightningShuffleboard.setDoubleSupplier("Indexer", "Power", () -> motor.get()); LightningShuffleboard.setBoolSupplier("Indexer", "EntryBeamBreak", () -> getEntryBeamBreakState()); LightningShuffleboard.setBoolSupplier("Indexer", "ExitBeamBreak", () -> getExitBeamBreakState()); @@ -114,7 +114,7 @@ public void setPieceState(PieceState state) { */ public void setPower(double power) { targetPower = power; - indexerMotor.setControl(dutyCycleControl.withOutput(power)); + motor.setControl(dutyCycleControl.withOutput(power)); } /** @@ -123,7 +123,7 @@ public void setPower(double power) { * @return current power of the indexer motor */ public double getPower() { - return indexerMotor.get(); + return motor.get(); } /** @@ -200,7 +200,7 @@ public void clearHasShot() { * @return current power of the indexer motor */ public double getIndexerPower() { - return indexerMotor.get(); + return motor.get(); } @Override @@ -227,7 +227,7 @@ public void periodic() { * update logging */ public void updateLogging() { - indexerPowerLog.append(indexerMotor.get()); + indexerPowerLog.append(motor.get()); indexerTargetPowerLog.append(targetPower); entryBeamBreakLog.append(getEntryBeamBreakState()); exitBeamBreakLog.append(getExitBeamBreakState()); From ff5eaa5166a7f2dcc568fef93e8592d6a3b35f49 Mon Sep 17 00:00:00 2001 From: Nate Bailey Date: Sat, 23 Mar 2024 09:14:28 -0400 Subject: [PATCH 24/38] [#485] retune shot maps --- src/main/java/frc/robot/Constants.java | 32 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 8 ++--- .../frc/robot/command/shoot/SmartShoot.java | 1 + 3 files changed, 21 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 72e98534..78c3137c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -528,7 +528,7 @@ public class RhapsodyPivotConstants { // TODO: get real public static final double MAX_INDEX_ANGLE = 40d; - public static final double MIN_ANGLE = 27d; + public static final double MIN_ANGLE = 25d; public static final double MAX_ANGLE = 105d; public static final double PIVOT_SYSTEST_ANGLE = 90d; @@ -561,8 +561,8 @@ public class ShooterConstants { put(2.5d, 3000d); put(3d, 3500d); put(3.5d, 4000d); - put(4.09d, 3600d); - put(4.86d, 4600d); + put(4d, 3600d); + put(4.75d, 4600d); } }; @@ -570,13 +570,13 @@ public class ShooterConstants { public static final InterpolationMap STEALTH_ANGLE_MAP = new InterpolationMap() { { // As distance gets smaller angle goes up - put(1.21d, 52d); - put(2d, 45d); - put(2.5d, 41.5d); - put(3d, 37d); - put(3.5d, 30d); - put(4.09d, 26.5d); - put(4.86, 26.5d); + put(1.21d, 50d); + put(2d, 43d); + put(2.5d, 39.5d); + put(3d, 35d); + put(3.5d, 30.5d); + put(4d, 29.5d); + put(4.75d, 28d); } }; @@ -588,9 +588,9 @@ public class ShooterConstants { put(2d, 2500d); put(2.5d, 3000d); put(3d, 3500d); - put(3.5d, 4000d); - put(4.09d, 3600d); - put(4.86d, 4600d); + put(3.5d, 3700d); + put(4d, 3700d); + put(4.75d, 4000d); } }; @@ -607,7 +607,7 @@ public class CandConstants { // TODO get real // PointBlank public static final double POINT_BLANK_RPM = 2000; - public static final double POINT_BLANK_ANGLE = 60; + public static final double POINT_BLANK_ANGLE = 50; // Podium public static final double PODIUM_RPM = 3500; @@ -675,7 +675,7 @@ public class ClimbConstants { // TODO: find real values public class LEDsConstants { - public static final int LED_LENGTH = 28; + public static final int LED_LENGTH = 29; public static final Map STRAND_START = new HashMap() { { @@ -689,7 +689,7 @@ public class LEDsConstants { { put(-1, LEDsConstants.LED_LENGTH); put(0, 14); - put(1, 14); + put(1, 15); } }; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0702ac82..4b60dda5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -241,15 +241,15 @@ protected void configureButtonBindings() { // .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, flywheel)) // cand shots for the robot - new Trigger(coPilot::getAButton) - .whileTrue(new AmpShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + // new Trigger(coPilot::getAButton) + // .whileTrue(new AmpShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); new Trigger(coPilot::getXButton) .whileTrue(new PointBlankShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); // new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, // pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot)); - // new Trigger(coPilot::getAButton).whileTrue(new Tune(flywheel, - // pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + new Trigger(coPilot::getAButton).whileTrue(new Tune(flywheel, + pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); /* BIAS */ new Trigger(() -> coPilot.getPOV() == 0) diff --git a/src/main/java/frc/robot/command/shoot/SmartShoot.java b/src/main/java/frc/robot/command/shoot/SmartShoot.java index c07833e8..d21d8f81 100644 --- a/src/main/java/frc/robot/command/shoot/SmartShoot.java +++ b/src/main/java/frc/robot/command/shoot/SmartShoot.java @@ -103,6 +103,7 @@ public void execute() { case SHOT: // Provides driver feed back and ends command new TimedCommand(RobotContainer.hapticDriverCommand(), 1d).schedule(); + new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); leds.enableState(LED_STATES.SHOT).withTimeout(2).schedule(); break; } From d484f6761dd0653bb049a72717cbfe97f538c007 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 23 Mar 2024 09:24:55 -0400 Subject: [PATCH 25/38] [#488] Tuned Amp limit --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 72e98534..17ba7d89 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -177,7 +177,7 @@ public static class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final double kSlipCurrentA = 300d; + private static final double kSlipCurrentA = 50d; // Theoretical free speed (m/s) at 12v applied output; // This needs to be tuned to your individual robot From c8bffab9a69dffbf4d06487c0bf4721d88f2ddb3 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 23 Mar 2024 10:22:06 -0400 Subject: [PATCH 26/38] [#486] Reenable climb --- src/main/java/frc/robot/RobotContainer.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9bf91040..af2a4671 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -89,7 +89,7 @@ public class RobotContainer extends LightningContainer { private Flywheel flywheel; private Pivot pivot; private Indexer indexer; - // private Climber climber; + private Climber climber; LEDs leds; Orchestra sing; @@ -130,7 +130,7 @@ protected void initializeSubsystems() { flywheel = new Flywheel(); pivot = Constants.isMercury() ? new PivotMercury() : new PivotRhapsody(); indexer = new Indexer(collector); - // climber = new Climber(); + climber = new Climber(); leds = new LEDs(); sing = new Orchestra(); @@ -249,14 +249,14 @@ protected void configureButtonBindings() { // pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot)); // new Trigger(coPilot::getAButton).whileTrue(new Tune(flywheel, - // pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + // pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); if (Constants.isMercury()) { new Trigger(coPilot::getAButton).whileTrue(new ReverseAmpShot(flywheel, pivot)); } else { new Trigger(coPilot::getAButton) - .whileTrue(new AmpShot(flywheel, - pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + .whileTrue(new AmpShot(flywheel, + pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); } /* BIAS */ @@ -361,8 +361,7 @@ protected void configureDefaultCommands() { // () -> -coPilot.getRightY(), // coPilot::getYButton).deadlineWith(leds.enableState(LED_STATES.CLIMBING))); - // climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> - // -coPilot.getRightY(), climber)); + climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber)); } protected Command getAutonomousCommand() { From 5952e505817db9ac3e9f3643a074207473214557 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 23 Mar 2024 10:25:45 -0400 Subject: [PATCH 27/38] [#486] Comment --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eaaa79eb..a074a921 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -625,7 +625,7 @@ public class CandConstants { // TODO get real public static final double C3_RPM = 0; public static final double C3_ANGLE = 0; - // Mercury Amp Shot + // Reverse Amp Shot public static final double REVERSE_AMP_TOP_RPM = 500; public static final double REVERSE_AMP_BOTTOM_RPM = 650; public static final double REVERSE_AMP_ANGLE = 110; From 470d63053c33a0173c9cee6eae3901d612e23074 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 23 Mar 2024 10:27:56 -0400 Subject: [PATCH 28/38] [#486] Make linter happier --- .github/sun_checks.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/sun_checks.xml b/.github/sun_checks.xml index 7fd23ed9..8f3814e3 100644 --- a/.github/sun_checks.xml +++ b/.github/sun_checks.xml @@ -137,7 +137,9 @@ - + + + From 0da36cfc789ec280a7e5e0af602ee94e9d52f46d Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 23 Mar 2024 14:17:48 -0400 Subject: [PATCH 29/38] [#123] Update Pathplanner Lib --- vendordeps/PathplannerLib.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index b849e926..a0197060 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.5", + "version": "2024.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.5" + "version": "2024.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.5", + "version": "2024.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 1d2c3dcad6c7dcd7637f0622b3fc1a41b56476d7 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 23 Mar 2024 14:27:23 -0400 Subject: [PATCH 30/38] [#491] Auton point at tag --- src/main/java/frc/robot/Constants.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 37 ++++++++++++++----- .../frc/robot/command/AutonPointAtTag.java | 22 +++++++++++ .../java/frc/robot/command/PointAtTag.java | 30 +++++++-------- .../frc/robot/command/SetStopMePipeline.java | 37 +++++++++++++++++++ 5 files changed, 103 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/robot/command/AutonPointAtTag.java create mode 100644 src/main/java/frc/robot/command/SetStopMePipeline.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 517fa618..1dd476ba 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -321,7 +321,8 @@ 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 PIDController TAG_AIM_CONTROLLER = new PIDController(0.2, 0, 0, 0.01); + 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 PIDController CHASE_CONTROLLER = new PIDController(0.05, 0, 0); public static final int TAG_PIPELINE = 0; public static final int SPEAKER_PIPELINE = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4b60dda5..45b55607 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,8 @@ import frc.robot.Constants.PathFindingConstants; import frc.robot.Constants.LEDsConstants.LED_STATES; import frc.robot.Constants.TunerConstants; +import frc.robot.Constants.VisionConstants; +import frc.robot.command.AutonPointAtTag; import frc.robot.command.AutonSmartCollect; import frc.robot.command.ChasePieces; import frc.robot.command.Collect; @@ -38,6 +40,7 @@ import frc.robot.command.PointAtPoint; import frc.robot.command.PointAtTag; import frc.robot.command.SetPointClimb; +import frc.robot.command.SetStopMePipeline; import frc.robot.command.Sing; import frc.robot.command.SmartClimb; import frc.robot.command.SmartCollect; @@ -129,7 +132,9 @@ protected void initializeSubsystems() { flywheel = new Flywheel(); pivot = Constants.isMercury() ? new PivotMercury() : new PivotRhapsody(); indexer = new Indexer(collector); - climber = new Climber(); + if (!Constants.isMercury()) { + climber = new Climber(); + } leds = new LEDs(); sing = new Orchestra(); @@ -176,6 +181,9 @@ protected void initializeNamedCommands() { NamedCommands.registerCommand("Has-Piece", new HasPieceAuto(indexer)); NamedCommands.registerCommand("Stop-Drive", new stopDrive(drivetrain)); NamedCommands.registerCommand("Stop-Flywheel", new FlywheelIN(flywheel)); + NamedCommands.registerCommand("Stopme-Tag", new SetStopMePipeline(limelights, VisionConstants.TAG_PIPELINE)); + NamedCommands.registerCommand("Stopme-Speaker", new SetStopMePipeline(limelights, VisionConstants.SPEAKER_PIPELINE)); + NamedCommands.registerCommand("Point-At-Tag", new AutonPointAtTag(drivetrain, limelights, driver)); // make sure named commands are initialized before autobuilder! autoChooser = AutoBuilder.buildAutoChooser(); @@ -224,8 +232,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 AutonPointAtTag(drivetrain, limelights, driver)); // new Trigger(driver::getYButton) // .whileTrue(new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain)); @@ -237,19 +247,24 @@ protected void configureButtonBindings() { .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, flywheel)) .deadlineWith(leds.enableState(LED_STATES.COLLECTING))); - - // .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, flywheel)) + + // .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, + // flywheel)) // cand shots for the robot - // new Trigger(coPilot::getAButton) - // .whileTrue(new AmpShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + new Trigger(coPilot::getAButton) + .whileTrue(new AmpShot(flywheel, + pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); new Trigger(coPilot::getXButton) .whileTrue(new PointBlankShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); // new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, // pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); - new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot)); - new Trigger(coPilot::getAButton).whileTrue(new Tune(flywheel, - pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + // new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot)); + // new Trigger(coPilot::getAButton).whileTrue(new Tune(flywheel, + // pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + new Trigger(coPilot::getYButton) + .onTrue(new InstantCommand(() -> limelights.setStopMePipeline(1), limelights)); + // .onFalse(new InstantCommand(() -> limelights.setStopMePipeline(0), limelights)); /* BIAS */ new Trigger(() -> coPilot.getPOV() == 0) @@ -353,7 +368,9 @@ protected void configureDefaultCommands() { // () -> -coPilot.getRightY(), // coPilot::getYButton).deadlineWith(leds.enableState(LED_STATES.CLIMBING))); - climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber)); + if (!Constants.isMercury()) { + climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber)); + } } protected Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/command/AutonPointAtTag.java b/src/main/java/frc/robot/command/AutonPointAtTag.java new file mode 100644 index 00000000..ec53af91 --- /dev/null +++ b/src/main/java/frc/robot/command/AutonPointAtTag.java @@ -0,0 +1,22 @@ +// 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 frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.Limelights; +import frc.robot.subsystems.Swerve; +import frc.thunder.filter.XboxControllerFilter; + +public class AutonPointAtTag extends PointAtTag { + + public AutonPointAtTag(Swerve swerve, Limelights limelights, XboxControllerFilter driver) { + super(swerve, limelights, driver); + } + + @Override + public boolean isFinished() { + return Math.abs(limelights.getStopMe().getTargetX()) < VisionConstants.POINTATTAG_ALIGNMENT_TOLERANCE; + } +} diff --git a/src/main/java/frc/robot/command/PointAtTag.java b/src/main/java/frc/robot/command/PointAtTag.java index 7ff21f9b..195e0d35 100644 --- a/src/main/java/frc/robot/command/PointAtTag.java +++ b/src/main/java/frc/robot/command/PointAtTag.java @@ -9,13 +9,12 @@ import frc.robot.Constants.VisionConstants; import frc.robot.subsystems.Limelights; import frc.robot.subsystems.Swerve; -import frc.thunder.vision.Limelight; public class PointAtTag extends Command { - private Swerve drivetrain; - private Limelight limelight; - private XboxController driver; + protected Swerve drivetrain; + protected Limelights limelights; + protected XboxController driver; private double pidOutput; private double targetHeading; @@ -28,21 +27,16 @@ public class PointAtTag extends Command { /** * Creates a new PointAtTag. - * + * * @param drivetrain to request movement * @param limelights to get the limelight from * @param driver the driver's controller, used for drive input */ public PointAtTag(Swerve drivetrain, Limelights limelights, XboxController driver) { this.drivetrain = drivetrain; + this.limelights = limelights; this.driver = driver; - limelight = limelights.getStopMe(); - - if (limelight.getPipeline() != VisionConstants.SPEAKER_PIPELINE) { - limelight.setPipeline(VisionConstants.SPEAKER_PIPELINE); - } - addRequirements(drivetrain); initLogging(); @@ -50,6 +44,8 @@ public PointAtTag(Swerve drivetrain, Limelights limelights, XboxController drive @Override public void initialize() { + limelights.setStopMePipeline(VisionConstants.SPEAKER_PIPELINE); + headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); headingController.enableContinuousInput(-180, 180); @@ -69,8 +65,11 @@ private void initLogging() { public void execute() { previousTargetHeading = targetHeading; - targetHeading = limelight.getTargetX(); - pidOutput = headingController.calculate(0, targetHeading); + targetHeading = limelights.getStopMe().getTargetX(); + + if (limelights.getStopMePipeline() == VisionConstants.SPEAKER_PIPELINE) { + pidOutput = headingController.calculate(0, targetHeading); + } drivetrain.setFieldDriver( driver.getLeftY(), @@ -84,12 +83,13 @@ public void execute() { * update logging */ public void updateLogging() { - deltaYLog.append(limelight.getTargetY()); - deltaXLog.append(limelight.getTargetX()); + deltaYLog.append(limelights.getStopMe().getTargetY()); + deltaXLog.append(limelights.getStopMe().getTargetX()); } @Override public void end(boolean interrupted) { + limelights.setStopMePipeline(VisionConstants.TAG_PIPELINE); } /** diff --git a/src/main/java/frc/robot/command/SetStopMePipeline.java b/src/main/java/frc/robot/command/SetStopMePipeline.java new file mode 100644 index 00000000..3db0e4bf --- /dev/null +++ b/src/main/java/frc/robot/command/SetStopMePipeline.java @@ -0,0 +1,37 @@ +// 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.wpilibj2.command.Command; +import frc.robot.subsystems.Limelights; + +public class SetStopMePipeline extends Command { + + private Limelights limelights; + private int pipeline; + + public SetStopMePipeline(Limelights limelights, int pipeline) { + this.limelights = limelights; + this.pipeline = pipeline; + } + + @Override + public void initialize() { + limelights.getStopMe().setPipeline(pipeline); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return false; + } +} From ca969b5f9f88cc22ec2f3b608ed384fe185e00cb Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 23 Mar 2024 14:32:33 -0400 Subject: [PATCH 31/38] [#491] Remove testing --- src/main/java/frc/robot/RobotContainer.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7041ae50..f1b30585 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -233,10 +233,8 @@ 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 AutonPointAtTag(drivetrain, limelights, driver)); + .whileTrue(new PointAtPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver)); // new Trigger(driver::getYButton) // .whileTrue(new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain)); From eeae904dbbd4d34c8c0392a2344f48e133670119 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Mon, 25 Mar 2024 18:31:58 -0400 Subject: [PATCH 32/38] [#497] changed any calling of the SetStopMePipeline command --- src/main/java/frc/robot/RobotContainer.java | 5 +-- .../java/frc/robot/command/PointAtTag.java | 3 +- .../frc/robot/command/SetStopMePipeline.java | 37 ------------------- 3 files changed, 4 insertions(+), 41 deletions(-) delete mode 100644 src/main/java/frc/robot/command/SetStopMePipeline.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f1b30585..82615195 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,6 @@ import frc.robot.command.PointAtPoint; import frc.robot.command.PointAtTag; import frc.robot.command.SetPointClimb; -import frc.robot.command.SetStopMePipeline; import frc.robot.command.Sing; import frc.robot.command.SmartClimb; import frc.robot.command.SmartCollect; @@ -182,8 +181,8 @@ protected void initializeNamedCommands() { NamedCommands.registerCommand("Has-Piece", new HasPieceAuto(indexer)); NamedCommands.registerCommand("Stop-Drive", new stopDrive(drivetrain)); NamedCommands.registerCommand("Stop-Flywheel", new FlywheelIN(flywheel)); - NamedCommands.registerCommand("Stopme-Tag", new SetStopMePipeline(limelights, VisionConstants.TAG_PIPELINE)); - NamedCommands.registerCommand("Stopme-Speaker", new SetStopMePipeline(limelights, VisionConstants.SPEAKER_PIPELINE)); + NamedCommands.registerCommand("Stopme-Tag", new InstantCommand(() -> limelights.setStopMePipeline(VisionConstants.TAG_PIPELINE))); + NamedCommands.registerCommand("Stopme-Speaker", new InstantCommand(() -> limelights.setStopMePipeline(VisionConstants.SPEAKER_PIPELINE))); NamedCommands.registerCommand("Point-At-Tag", new AutonPointAtTag(drivetrain, limelights, driver)); // make sure named commands are initialized before autobuilder! diff --git a/src/main/java/frc/robot/command/PointAtTag.java b/src/main/java/frc/robot/command/PointAtTag.java index 195e0d35..50312b04 100644 --- a/src/main/java/frc/robot/command/PointAtTag.java +++ b/src/main/java/frc/robot/command/PointAtTag.java @@ -6,6 +6,7 @@ import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.vision.VisionPipeline; import frc.robot.Constants.VisionConstants; import frc.robot.subsystems.Limelights; import frc.robot.subsystems.Swerve; @@ -44,7 +45,7 @@ public PointAtTag(Swerve drivetrain, Limelights limelights, XboxController drive @Override public void initialize() { - limelights.setStopMePipeline(VisionConstants.SPEAKER_PIPELINE); + limelights.setStopMePipeline(VisionConstants.SPEAKER_PIPELINE);; headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); diff --git a/src/main/java/frc/robot/command/SetStopMePipeline.java b/src/main/java/frc/robot/command/SetStopMePipeline.java deleted file mode 100644 index 3db0e4bf..00000000 --- a/src/main/java/frc/robot/command/SetStopMePipeline.java +++ /dev/null @@ -1,37 +0,0 @@ -// 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.wpilibj2.command.Command; -import frc.robot.subsystems.Limelights; - -public class SetStopMePipeline extends Command { - - private Limelights limelights; - private int pipeline; - - public SetStopMePipeline(Limelights limelights, int pipeline) { - this.limelights = limelights; - this.pipeline = pipeline; - } - - @Override - public void initialize() { - limelights.getStopMe().setPipeline(pipeline); - } - - @Override - public void execute() { - } - - @Override - public void end(boolean interrupted) { - } - - @Override - public boolean isFinished() { - return false; - } -} From 3c322e5a41942d18b7b721597c378737311e119f Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Mon, 25 Mar 2024 18:37:31 -0400 Subject: [PATCH 33/38] [#497] fixed linter errors (a single extra semicolon ^>:( ) --- src/main/java/frc/robot/command/PointAtTag.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/command/PointAtTag.java b/src/main/java/frc/robot/command/PointAtTag.java index 50312b04..17c0ca08 100644 --- a/src/main/java/frc/robot/command/PointAtTag.java +++ b/src/main/java/frc/robot/command/PointAtTag.java @@ -45,7 +45,7 @@ public PointAtTag(Swerve drivetrain, Limelights limelights, XboxController drive @Override public void initialize() { - limelights.setStopMePipeline(VisionConstants.SPEAKER_PIPELINE);; + limelights.setStopMePipeline(VisionConstants.SPEAKER_PIPELINE); headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); From 36f92feead6ad714f18ee6bae55ec8db2affc455 Mon Sep 17 00:00:00 2001 From: diedinvane Date: Mon, 25 Mar 2024 19:56:44 -0400 Subject: [PATCH 34/38] [#493] clean pipeline calls --- src/main/java/frc/robot/Constants.java | 11 +++++------ src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/command/PointAtTag.java | 6 +++--- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b244294e..4c66fac1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -324,9 +324,7 @@ public class VisionConstants { 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 PIDController CHASE_CONTROLLER = new PIDController(0.05, 0, 0); - public static final int TAG_PIPELINE = 0; - public static final int SPEAKER_PIPELINE = 1; - public static final int NOTE_PIPELINE = 2; + public static final double HALF_FIELD_HEIGHT = Units.feetToMeters(13); @@ -334,9 +332,10 @@ public class VisionConstants { public static final Translation3d RED_SPEAKER_LOCATION = new Translation3d(16.4592, 5.547593, 1.2); public class Pipelines { // TODO get real - public static final int APRIL_TAG_3d = 0; - public static final int APRIL_TAG_2d = 1; - public static final int CHASE_PIECE = 2; // FOR the collector + 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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 82615195..17e8f7d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -181,8 +181,8 @@ protected void initializeNamedCommands() { NamedCommands.registerCommand("Has-Piece", new HasPieceAuto(indexer)); NamedCommands.registerCommand("Stop-Drive", new stopDrive(drivetrain)); NamedCommands.registerCommand("Stop-Flywheel", new FlywheelIN(flywheel)); - NamedCommands.registerCommand("Stopme-Tag", new InstantCommand(() -> limelights.setStopMePipeline(VisionConstants.TAG_PIPELINE))); - NamedCommands.registerCommand("Stopme-Speaker", new InstantCommand(() -> limelights.setStopMePipeline(VisionConstants.SPEAKER_PIPELINE))); + NamedCommands.registerCommand("Stopme-Tag", new InstantCommand(() -> limelights.setStopMePipeline(VisionConstants.Pipelines.TAG_PIPELINE))); + NamedCommands.registerCommand("Stopme-Speaker", new InstantCommand(() -> limelights.setStopMePipeline(VisionConstants.Pipelines.SPEAKER_PIPELINE))); NamedCommands.registerCommand("Point-At-Tag", new AutonPointAtTag(drivetrain, limelights, driver)); // make sure named commands are initialized before autobuilder! diff --git a/src/main/java/frc/robot/command/PointAtTag.java b/src/main/java/frc/robot/command/PointAtTag.java index 17c0ca08..6cc23726 100644 --- a/src/main/java/frc/robot/command/PointAtTag.java +++ b/src/main/java/frc/robot/command/PointAtTag.java @@ -45,7 +45,7 @@ public PointAtTag(Swerve drivetrain, Limelights limelights, XboxController drive @Override public void initialize() { - limelights.setStopMePipeline(VisionConstants.SPEAKER_PIPELINE); + limelights.setStopMePipeline(VisionConstants.Pipelines.SPEAKER_PIPELINE); headingController.setTolerance(VisionConstants.ALIGNMENT_TOLERANCE); @@ -68,7 +68,7 @@ public void execute() { targetHeading = limelights.getStopMe().getTargetX(); - if (limelights.getStopMePipeline() == VisionConstants.SPEAKER_PIPELINE) { + if (limelights.getStopMePipeline() == VisionConstants.Pipelines.SPEAKER_PIPELINE) { pidOutput = headingController.calculate(0, targetHeading); } @@ -90,7 +90,7 @@ public void updateLogging() { @Override public void end(boolean interrupted) { - limelights.setStopMePipeline(VisionConstants.TAG_PIPELINE); + limelights.setStopMePipeline(VisionConstants.Pipelines.TAG_PIPELINE); } /** From eb47a582ce210b81234c17c182345605dc4e2d85 Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Mon, 25 Mar 2024 20:48:02 -0400 Subject: [PATCH 35/38] [#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 36/38] [#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 From df3af8cf88167031bda999e259001de9e3751b78 Mon Sep 17 00:00:00 2001 From: Nate Bailey Date: Mon, 25 Mar 2024 21:08:25 -0400 Subject: [PATCH 37/38] [#501] update constants --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4c66fac1..bb4f8060 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -626,9 +626,9 @@ public class CandConstants { // TODO get real public static final double C3_ANGLE = 0; // Reverse Amp Shot - public static final double REVERSE_AMP_TOP_RPM = 500; - public static final double REVERSE_AMP_BOTTOM_RPM = 650; - public static final double REVERSE_AMP_ANGLE = 110; + public static final double REVERSE_AMP_TOP_RPM = 300; + public static final double REVERSE_AMP_BOTTOM_RPM = 700; + public static final double REVERSE_AMP_ANGLE = 93; // Line public static final double LINE_RPM = 0; From ced0684ffe8b588f1a12206e1e60aa790ed2edad Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Tue, 26 Mar 2024 15:30:28 -0400 Subject: [PATCH 38/38] [#501] Singular amp --- src/main/java/frc/robot/Constants.java | 11 ++--- src/main/java/frc/robot/RobotContainer.java | 11 ++--- .../robot/command/shoot/ReverseAmpShot.java | 45 ------------------- 3 files changed, 6 insertions(+), 61 deletions(-) delete mode 100644 src/main/java/frc/robot/command/shoot/ReverseAmpShot.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bb4f8060..74119d6a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -601,9 +601,9 @@ public enum ShootingState { public class CandConstants { // TODO get real // Amp - public static final double AMP_TOP_RPM = 50; - public static final double AMP_BOTTOM_RPM = 2000; - public static final double AMP_ANGLE = 45; + public static final double AMP_TOP_RPM = 300; + public static final double AMP_BOTTOM_RPM = 700; + public static final double AMP_ANGLE = 93; // PointBlank public static final double POINT_BLANK_RPM = 2000; @@ -625,11 +625,6 @@ public class CandConstants { // TODO get real public static final double C3_RPM = 0; public static final double C3_ANGLE = 0; - // Reverse Amp Shot - public static final double REVERSE_AMP_TOP_RPM = 300; - public static final double REVERSE_AMP_BOTTOM_RPM = 700; - public static final double REVERSE_AMP_ANGLE = 93; - // Line public static final double LINE_RPM = 0; public static final double LINE_ANGLE = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17e8f7d9..0eb07306 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -46,7 +46,6 @@ import frc.robot.command.stopDrive; import frc.robot.command.shoot.AmpShot; import frc.robot.command.shoot.FlywheelIN; -import frc.robot.command.shoot.ReverseAmpShot; import frc.robot.command.shoot.PointBlankShot; import frc.robot.command.shoot.SmartShoot; import frc.robot.command.shoot.PivotUP; @@ -258,13 +257,9 @@ protected void configureButtonBindings() { // new Trigger(coPilot::getAButton).whileTrue(new Tune(flywheel, // pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); - if (Constants.isMercury()) { - new Trigger(coPilot::getAButton).whileTrue(new ReverseAmpShot(flywheel, pivot)); - } else { - new Trigger(coPilot::getAButton) - .whileTrue(new AmpShot(flywheel, pivot) - .deadlineWith(leds.enableState(LED_STATES.SHOOTING))); - } + new Trigger(coPilot::getAButton) + .whileTrue(new AmpShot(flywheel, pivot) + .deadlineWith(leds.enableState(LED_STATES.SHOOTING))); /* BIAS */ new Trigger(() -> coPilot.getPOV() == 0) diff --git a/src/main/java/frc/robot/command/shoot/ReverseAmpShot.java b/src/main/java/frc/robot/command/shoot/ReverseAmpShot.java deleted file mode 100644 index 8d771cd5..00000000 --- a/src/main/java/frc/robot/command/shoot/ReverseAmpShot.java +++ /dev/null @@ -1,45 +0,0 @@ -// 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.shoot; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.CandConstants; -import frc.robot.subsystems.Flywheel; -import frc.robot.subsystems.Pivot; - -public class ReverseAmpShot extends Command { - - private final Flywheel flywheel; - private final Pivot pivot; - - public ReverseAmpShot(Flywheel flywheel, Pivot pivot) { - this.flywheel = flywheel; - this.pivot = pivot; - - addRequirements(flywheel, pivot); - } - - @Override - public void initialize() { - } - - @Override - public void execute() { - pivot.setTargetAngle(CandConstants.REVERSE_AMP_ANGLE + pivot.getBias()); - flywheel.setTopMotorRPM(CandConstants.REVERSE_AMP_TOP_RPM + flywheel.getBias()); - flywheel.setBottomMotorRPM(CandConstants.REVERSE_AMP_BOTTOM_RPM + flywheel.getBias()); - } - - @Override - public void end(boolean interrupted) { - pivot.setTargetAngle(pivot.getStowAngle()); - flywheel.coast(true); - } - - @Override - public boolean isFinished() { - return false; - } -}