From d7898e1538aab04c5676a5afa85cb3e2c5b85d92 Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Wed, 20 Mar 2024 19:36:27 -0400 Subject: [PATCH 1/9] Few small changes have been made --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- .../commands/shooter/{ShootAmp.java => ShootTrap.java} | 10 +++++----- src/main/java/frc/robot/constants/GameConstants.java | 6 +++--- src/main/java/frc/robot/subsystems/Ramp.java | 2 +- 4 files changed, 13 insertions(+), 13 deletions(-) rename src/main/java/frc/robot/commands/shooter/{ShootAmp.java => ShootTrap.java} (74%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61274ddf..63f1c9f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -234,10 +234,10 @@ private void configureBindings() { new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_SPEAKER_AWAY), new ShootSpeaker(shooter, drivetrain, lightStrip))); - // Set up to shoot AMP - A - controller.a().onTrue(CommandUtil.parallel("Setup Amp shot", - new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_AMP), - new ShootAmp(shooter, lightStrip))); + // Set up to shoot TRAP - A + controller.a().onTrue(CommandUtil.parallel("Setup Trap shot", + new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_TRAP), //Needs to be set + new ShootTrap(shooter, lightStrip))); // Cancell all - B controller.b().onTrue(CommandUtil.logged(new CancelAll(ramp, shooter, lightStrip))); diff --git a/src/main/java/frc/robot/commands/shooter/ShootAmp.java b/src/main/java/frc/robot/commands/shooter/ShootTrap.java similarity index 74% rename from src/main/java/frc/robot/commands/shooter/ShootAmp.java rename to src/main/java/frc/robot/commands/shooter/ShootTrap.java index 0ea299c8..b18aac12 100644 --- a/src/main/java/frc/robot/commands/shooter/ShootAmp.java +++ b/src/main/java/frc/robot/commands/shooter/ShootTrap.java @@ -6,11 +6,11 @@ import frc.robot.subsystems.Shooter; import frc.robot.utils.BlinkinPattern; -public class ShootAmp extends Command { +public class ShootTrap extends Command { private final Shooter shooter; private final LightStrip lightStrip; - public ShootAmp(Shooter shooter, LightStrip lightStrip) { + public ShootTrap(Shooter shooter, LightStrip lightStrip) { this.shooter = shooter; this.lightStrip = lightStrip; addRequirements(shooter); @@ -27,9 +27,9 @@ public boolean isFinished() { @Override public void execute() { - shooter.setShooterMotorRightRPM(Constants.SHOOTER_MOTOR_AMP_SPEED); - shooter.setShooterMotorLeftRPM(Constants.SHOOTER_MOTOR_AMP_SPEED); - if (shooter.upToSpeed(Constants.SHOOTER_MOTOR_AMP_SPEED, Constants.SHOOTER_MOTOR_AMP_SPEED)){ + shooter.setShooterMotorRightRPM(Constants.SHOOTER_MOTOR_TRAP_SPEED); + shooter.setShooterMotorLeftRPM(Constants.SHOOTER_MOTOR_TRAP_SPEED); + if (shooter.upToSpeed(Constants.SHOOTER_MOTOR_TRAP_SPEED, Constants.SHOOTER_MOTOR_TRAP_SPEED)){ lightStrip.setPattern(BlinkinPattern.COLOR_WAVES_LAVA_PALETTE); } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 50bd0748..dfcf1935 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -51,7 +51,7 @@ public class GameConstants { public static final double RAMP_POS_STOW = 0.5; public static final double RAMP_POS_SHOOT_SPEAKER_CLOSE = 0.1; public static final double RAMP_POS_SHOOT_SPEAKER_AWAY = 7.8; //when about 44" away from the speaker - public static final double RAMP_POS_SHOOT_AMP = 7.0; + public static final double RAMP_POS_SHOOT_TRAP = 7.0; //SERVO public static final int RIGHT_SERVO_ENGAGED = 0; @@ -63,7 +63,7 @@ public class GameConstants { public static final double SHOOTER_MOTOR_SPEED_TRESHOLD = 100; //TODO: Refine This Number public static final double SHOOTER_MOTOR_LOW_SPEED = 3500; //multiplied power by 5000, need to refine later public static final double SHOOTER_MOTOR_HIGH_SPEED = 5500; //multiplied power by 5000, need to refine later - public static final double SHOOTER_MOTOR_AMP_SPEED = 1100; //multiplied power by 5000, need to refine later + public static final double SHOOTER_MOTOR_TRAP_SPEED = 2800; public static final double SHOOTER_MOTOR_1_RPM = 12000; public static final double SHOOTER_MOTOR_2_RPM = 12000; public static final double SHOOTER_TIME_AFTER_TRIGGER = 3; @@ -81,7 +81,7 @@ public class GameConstants { public static final double FEEDER_SLOW_SPEED = 0.3; public static final double FEEDER_BACK_DRIVE_SPEED = -0.25; public static final double FEEDER_MOTOR_SPEAKER_SPEED = 1.0; - public static final double FEEDER_MOTOR_AMP_SPEED = 0.7; + public static final double FEEDER_MOTOR_TRAP_SPEED = 0.8; public static final double FEEDER_BACK_DRIVE_TIMEOUT = 10.0; public static final double FEEDER_GAMEPIECE_UNTIL_LEAVE_TIMEOUT = 5.0; public static final double START_FEEDER_TIMEOUT = 5.0; diff --git a/src/main/java/frc/robot/subsystems/Ramp.java b/src/main/java/frc/robot/subsystems/Ramp.java index 954fe958..b07831c6 100644 --- a/src/main/java/frc/robot/subsystems/Ramp.java +++ b/src/main/java/frc/robot/subsystems/Ramp.java @@ -106,7 +106,7 @@ public boolean isShootAwayAngle(){ } public boolean isShootAmpAngle(){ - return (Math.abs(Constants.RAMP_POS_SHOOT_AMP - getRampPos()) <= Constants.RAMP_POS_THRESHOLD); + return (Math.abs(Constants.RAMP_POS_SHOOT_TRAP - getRampPos()) <= Constants.RAMP_POS_THRESHOLD); } public static double angleToEncoder(double angle){ //(y-b)/m=x From 4df0868bc54b9db366cd87d07169a2ea77f22e48 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Wed, 20 Mar 2024 21:27:12 -0400 Subject: [PATCH 2/9] Added the logic --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../frc/robot/constants/TagConstants.java | 86 +++++++++++++++++++ 2 files changed, 91 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/constants/TagConstants.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 63f1c9f8..36659a72 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,6 +50,7 @@ import frc.robot.commands.shooter.*; import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; +import frc.robot.constants.TagConstants; import frc.robot.subsystems.*; import frc.robot.swervev2.KinematicsConversionConfig; import frc.robot.swervev2.SwerveIdConfig; @@ -57,6 +58,7 @@ import frc.robot.utils.Alignable; import frc.robot.utils.Gain; import frc.robot.utils.PID; +import frc.robot.utils.PathPlannerUtils; import frc.robot.utils.logging.CommandUtil; import frc.robot.utils.smartshuffleboard.SmartShuffleboard; @@ -235,9 +237,9 @@ private void configureBindings() { new ShootSpeaker(shooter, drivetrain, lightStrip))); // Set up to shoot TRAP - A - controller.a().onTrue(CommandUtil.parallel("Setup Trap shot", - new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_TRAP), //Needs to be set - new ShootTrap(shooter, lightStrip))); + controller.a().onTrue(PathPlannerUtils.pathToPose(TagConstants.getTrapPosition(drivetrain), 0)); + // new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_TRAP), //Needs to be set + // new ShootTrap(shooter, lightStrip))); // Cancell all - B controller.b().onTrue(CommandUtil.logged(new CancelAll(ramp, shooter, lightStrip))); diff --git a/src/main/java/frc/robot/constants/TagConstants.java b/src/main/java/frc/robot/constants/TagConstants.java new file mode 100644 index 00000000..c3bfae70 --- /dev/null +++ b/src/main/java/frc/robot/constants/TagConstants.java @@ -0,0 +1,86 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.SwerveDrivetrain; + +public class TagConstants { + private static final double X_ADJUSTMENT = 10.625; + private static final double Y_ADJUSTMENT = 18.4; + private static final double FULL_ADJUMENT = 21.25; + private static double lowest; + private static TrapPositionList lowestTag; + + enum TrapPositionList { + ELEVEN(11, 468.660 + X_ADJUSTMENT, 146.063 - Y_ADJUSTMENT, 120), + TWELVE(12, 468.660 + X_ADJUSTMENT, 177.099 + Y_ADJUSTMENT, 240), + THIRTEEN(13, 441.739 - FULL_ADJUMENT, 161.620, 0), + FOURTEEN(14, 209.480 + FULL_ADJUMENT, 161.620, 180), + FIFTEEN(15, 182.72 - X_ADJUSTMENT, 177.099 + Y_ADJUSTMENT, 300), + SIXTEEN(16, 182.72 - X_ADJUSTMENT, 146.063 - Y_ADJUSTMENT, 60); + + private final int tagId; + private final double xPos; + private final double yPos; + private final double rot; + + + TrapPositionList(int tagId, double xPos, double yPos, double rot) { + this.tagId = tagId; + this.xPos = xPos; + this.yPos = yPos; + this.rot = rot; + } + public double getXPos() { return xPos;} + public double getYPos() { return yPos;} + public double getRot() { return rot;} + public double getTagID() {return tagId;} + } + + public static boolean setLowestTag(double currentLowestTag, TrapPositionList possibleLowestTag, Pose2d currentPose) { + return currentLowestTag > Math.sqrt(Math.pow((possibleLowestTag.getXPos() - currentPose.getX()), 2) + Math.pow(possibleLowestTag.getYPos() - currentPose.getY(), 2)); + } + + public static Pose2d getTrapPosition(SwerveDrivetrain drivetrain) { + double x = drivetrain.getPose().getX(); + double y = drivetrain.getPose().getY(); + + for (int i = 11; i > 16; i++) { + if (setLowestTag(lowest, lowestTag, drivetrain.getPose())) { + lowestTag = TrapPositionList.ELEVEN;//Help + } + } + + + lowestTag = TrapPositionList.ELEVEN; + + lowest = Math.sqrt(Math.pow((TrapPositionList.ELEVEN.getXPos() - x), 2) + Math.pow(TrapPositionList.TWELVE.getYPos() - y, 2)); + + if (lowest > (Math.sqrt(Math.pow((TrapPositionList.TWELVE.getXPos() - x), 2) + Math.pow(TrapPositionList.TWELVE.getYPos() - y, 2)))) { + lowestTag = TrapPositionList.TWELVE; + } + if (lowest > (Math.sqrt(Math.pow((TrapPositionList.THIRTEEN.getXPos() - x), 2) + Math.pow(TrapPositionList.THIRTEEN.getYPos() - y, 2)))) { + lowestTag = TrapPositionList.THIRTEEN; + } + if (lowest > (Math.sqrt(Math.pow((TrapPositionList.FOURTEEN.getXPos() - x), 2) + Math.pow(TrapPositionList.FOURTEEN.getYPos() - y, 2)))) { + lowestTag = TrapPositionList.FOURTEEN; + } + if (lowest > (Math.sqrt(Math.pow((TrapPositionList.FIFTEEN.getXPos() - x), 2) + Math.pow(TrapPositionList.FIFTEEN.getYPos() - y, 2)))) { + lowestTag = TrapPositionList.FIFTEEN; + } + if (lowest > (Math.sqrt(Math.pow((TrapPositionList.SIXTEEN.getXPos() - x), 2) + Math.pow(TrapPositionList.SIXTEEN.getYPos() - y, 2)))) { + lowestTag = TrapPositionList.SIXTEEN; + } + + return new Pose2d(lowestTag.getXPos(), lowestTag.getYPos(), new Rotation2d(lowestTag.getRot())); + + + // todo: + // Get current position + // Figure out which location is closest + // Return that target position + } + + public class getTrapPosition { + } +} From 99697d42e2765b5d50aa70b48094107a6434ece5 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Wed, 20 Mar 2024 21:59:43 -0400 Subject: [PATCH 3/9] Fixed --- .../frc/robot/constants/TagConstants.java | 35 +++---------------- 1 file changed, 4 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/constants/TagConstants.java b/src/main/java/frc/robot/constants/TagConstants.java index c3bfae70..8d774a03 100644 --- a/src/main/java/frc/robot/constants/TagConstants.java +++ b/src/main/java/frc/robot/constants/TagConstants.java @@ -47,40 +47,13 @@ public static Pose2d getTrapPosition(SwerveDrivetrain drivetrain) { for (int i = 11; i > 16; i++) { if (setLowestTag(lowest, lowestTag, drivetrain.getPose())) { - lowestTag = TrapPositionList.ELEVEN;//Help + lowestTag = TrapPositionList.values() [i]; + lowest = Math.sqrt(Math.pow((lowestTag.getXPos() - x), 2) + Math.pow(lowestTag.getYPos() - y, 2)); } } - - lowestTag = TrapPositionList.ELEVEN; - - lowest = Math.sqrt(Math.pow((TrapPositionList.ELEVEN.getXPos() - x), 2) + Math.pow(TrapPositionList.TWELVE.getYPos() - y, 2)); - - if (lowest > (Math.sqrt(Math.pow((TrapPositionList.TWELVE.getXPos() - x), 2) + Math.pow(TrapPositionList.TWELVE.getYPos() - y, 2)))) { - lowestTag = TrapPositionList.TWELVE; - } - if (lowest > (Math.sqrt(Math.pow((TrapPositionList.THIRTEEN.getXPos() - x), 2) + Math.pow(TrapPositionList.THIRTEEN.getYPos() - y, 2)))) { - lowestTag = TrapPositionList.THIRTEEN; - } - if (lowest > (Math.sqrt(Math.pow((TrapPositionList.FOURTEEN.getXPos() - x), 2) + Math.pow(TrapPositionList.FOURTEEN.getYPos() - y, 2)))) { - lowestTag = TrapPositionList.FOURTEEN; - } - if (lowest > (Math.sqrt(Math.pow((TrapPositionList.FIFTEEN.getXPos() - x), 2) + Math.pow(TrapPositionList.FIFTEEN.getYPos() - y, 2)))) { - lowestTag = TrapPositionList.FIFTEEN; - } - if (lowest > (Math.sqrt(Math.pow((TrapPositionList.SIXTEEN.getXPos() - x), 2) + Math.pow(TrapPositionList.SIXTEEN.getYPos() - y, 2)))) { - lowestTag = TrapPositionList.SIXTEEN; - } - return new Pose2d(lowestTag.getXPos(), lowestTag.getYPos(), new Rotation2d(lowestTag.getRot())); - - - // todo: - // Get current position - // Figure out which location is closest - // Return that target position - } - - public class getTrapPosition { } } + + From 2ea02783b4f4494404e0da2251514429604674eb Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Sat, 23 Mar 2024 11:37:34 -0400 Subject: [PATCH 4/9] Fixed Glaring Issues --- src/main/java/frc/robot/constants/GameConstants.java | 2 ++ src/main/java/frc/robot/constants/TagConstants.java | 8 ++++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index dfcf1935..9a43d881 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -6,6 +6,8 @@ public class GameConstants { public static final int RIGHT_JOYSTICK_ID = 1; public static final int XBOX_CONTROLLER_ID = 2; + public static final double METERS_TO_INCHES = 39.37; + public static final double AUTO_SPOOL_AND_SHOOT_TIME = 4; //DEBUGS public static final boolean SHOOTER_DEBUG = false; diff --git a/src/main/java/frc/robot/constants/TagConstants.java b/src/main/java/frc/robot/constants/TagConstants.java index 8d774a03..6d7e2777 100644 --- a/src/main/java/frc/robot/constants/TagConstants.java +++ b/src/main/java/frc/robot/constants/TagConstants.java @@ -38,21 +38,21 @@ enum TrapPositionList { } public static boolean setLowestTag(double currentLowestTag, TrapPositionList possibleLowestTag, Pose2d currentPose) { - return currentLowestTag > Math.sqrt(Math.pow((possibleLowestTag.getXPos() - currentPose.getX()), 2) + Math.pow(possibleLowestTag.getYPos() - currentPose.getY(), 2)); + return currentLowestTag > Math.sqrt(Math.pow((possibleLowestTag.getXPos() - currentPose.getX() * Constants.METERS_TO_INCHES), 2) + Math.pow(possibleLowestTag.getYPos() - currentPose.getY() * Constants.METERS_TO_INCHES, 2)); } public static Pose2d getTrapPosition(SwerveDrivetrain drivetrain) { double x = drivetrain.getPose().getX(); double y = drivetrain.getPose().getY(); - for (int i = 11; i > 16; i++) { + for (int i = 0; i >= 5; i++) { if (setLowestTag(lowest, lowestTag, drivetrain.getPose())) { lowestTag = TrapPositionList.values() [i]; - lowest = Math.sqrt(Math.pow((lowestTag.getXPos() - x), 2) + Math.pow(lowestTag.getYPos() - y, 2)); + lowest = Math.sqrt(Math.pow((lowestTag.getXPos() - x * Constants.METERS_TO_INCHES), 2) + Math.pow(lowestTag.getYPos() - y * Constants.METERS_TO_INCHES, 2)); } } - return new Pose2d(lowestTag.getXPos(), lowestTag.getYPos(), new Rotation2d(lowestTag.getRot())); + return new Pose2d(lowestTag.getXPos() / Constants.METERS_TO_INCHES, lowestTag.getYPos() / Constants.METERS_TO_INCHES, new Rotation2d(lowestTag.getRot())); } } From 2d9916fcb9041653b41378b1eb83bf9964a357fe Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Sat, 23 Mar 2024 12:12:13 -0400 Subject: [PATCH 5/9] Added Vision Trap Code --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/constants/TagConstants.java | 50 +++++++++++++------ 2 files changed, 36 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 36659a72..bf7a9405 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -237,7 +237,7 @@ private void configureBindings() { new ShootSpeaker(shooter, drivetrain, lightStrip))); // Set up to shoot TRAP - A - controller.a().onTrue(PathPlannerUtils.pathToPose(TagConstants.getTrapPosition(drivetrain), 0)); + controller.a().onTrue(PathPlannerUtils.pathToPose(TagConstants.getTrapPosition(), 0)); // new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_TRAP), //Needs to be set // new ShootTrap(shooter, lightStrip))); diff --git a/src/main/java/frc/robot/constants/TagConstants.java b/src/main/java/frc/robot/constants/TagConstants.java index 6d7e2777..f792afb1 100644 --- a/src/main/java/frc/robot/constants/TagConstants.java +++ b/src/main/java/frc/robot/constants/TagConstants.java @@ -2,14 +2,18 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.subsystems.SwerveDrivetrain; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; public class TagConstants { private static final double X_ADJUSTMENT = 10.625; private static final double Y_ADJUSTMENT = 18.4; private static final double FULL_ADJUMENT = 21.25; - private static double lowest; private static TrapPositionList lowestTag; + private static NetworkTableInstance inst = NetworkTableInstance.getDefault(); + private static NetworkTable table = inst.getTable("ROS"); + private static DoubleSubscriber subscriber = table.getDoubleTopic("apriltag_id").subscribe(0); enum TrapPositionList { ELEVEN(11, 468.660 + X_ADJUSTMENT, 146.063 - Y_ADJUSTMENT, 120), @@ -37,22 +41,38 @@ enum TrapPositionList { public double getTagID() {return tagId;} } - public static boolean setLowestTag(double currentLowestTag, TrapPositionList possibleLowestTag, Pose2d currentPose) { - return currentLowestTag > Math.sqrt(Math.pow((possibleLowestTag.getXPos() - currentPose.getX() * Constants.METERS_TO_INCHES), 2) + Math.pow(possibleLowestTag.getYPos() - currentPose.getY() * Constants.METERS_TO_INCHES, 2)); + public static void setLowestTag() { + + if (subscriber.get() == 11) { + lowestTag = TrapPositionList.ELEVEN; + } + if (subscriber.get() == 12) { + lowestTag = TrapPositionList.TWELVE; + } + if (subscriber.get() == 13) { + lowestTag = TrapPositionList.THIRTEEN; + } + if (subscriber.get() == 14) { + lowestTag = TrapPositionList.FOURTEEN; + } + if (subscriber.get() == 15) { + lowestTag = TrapPositionList.FIFTEEN; + } + if (subscriber.get() == 16) { + lowestTag = TrapPositionList.SIXTEEN; + } else { + lowestTag = null; + } + } - public static Pose2d getTrapPosition(SwerveDrivetrain drivetrain) { - double x = drivetrain.getPose().getX(); - double y = drivetrain.getPose().getY(); - - for (int i = 0; i >= 5; i++) { - if (setLowestTag(lowest, lowestTag, drivetrain.getPose())) { - lowestTag = TrapPositionList.values() [i]; - lowest = Math.sqrt(Math.pow((lowestTag.getXPos() - x * Constants.METERS_TO_INCHES), 2) + Math.pow(lowestTag.getYPos() - y * Constants.METERS_TO_INCHES, 2)); - } + public static Pose2d getTrapPosition() { + setLowestTag(); + if (lowestTag != null) { + return new Pose2d(lowestTag.getXPos() / Constants.METERS_TO_INCHES, lowestTag.getYPos() / Constants.METERS_TO_INCHES, new Rotation2d(lowestTag.getRot())); + } else { + return null; } - - return new Pose2d(lowestTag.getXPos() / Constants.METERS_TO_INCHES, lowestTag.getYPos() / Constants.METERS_TO_INCHES, new Rotation2d(lowestTag.getRot())); } } From b55904efca8689c123e34b1e057f4515f724e01a Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Sat, 23 Mar 2024 14:21:05 -0400 Subject: [PATCH 6/9] Noah's weird stuff --- src/main/java/frc/robot/RobotContainer.java | 7 +- .../java/frc/robot/constants/AprilTag.java | 37 +++++++++ .../frc/robot/constants/GameConstants.java | 3 + .../frc/robot/constants/TagConstants.java | 79 ------------------- .../frc/robot/utils/TrapPositionList.java | 52 ++++++++++++ 5 files changed, 97 insertions(+), 81 deletions(-) create mode 100644 src/main/java/frc/robot/constants/AprilTag.java delete mode 100644 src/main/java/frc/robot/constants/TagConstants.java create mode 100644 src/main/java/frc/robot/utils/TrapPositionList.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index df7c736f..0e79c86b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,9 +11,11 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -49,7 +51,7 @@ import frc.robot.commands.shooter.StopShooter; import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; -import frc.robot.constants.TagConstants; +import frc.robot.constants.AprilTag; import frc.robot.subsystems.*; import frc.robot.swervev2.KinematicsConversionConfig; import frc.robot.swervev2.SwerveIdConfig; @@ -93,6 +95,7 @@ public class RobotContainer { private final CommandXboxController controller = new CommandXboxController(Constants.XBOX_CONTROLLER_ID); private SwerveDrivetrain drivetrain; private AutoChooser2024 autoChooser; + private final AprilTag aprilTag = new AprilTag(); /**f * The container for the robot. Contains subsystems, OI devices, and commands. @@ -236,7 +239,7 @@ private void configureBindings() { new ShootSpeaker(shooter, drivetrain, lightStrip))); // Set up to shoot TRAP - A - controller.a().onTrue(PathPlannerUtils.pathToPose(TagConstants.getTrapPosition(), 0)); + controller.a().onTrue(new InstantCommand(()->aprilTag.planPath().schedule())); // new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_TRAP), //Needs to be set // new ShootTrap(shooter, lightStrip))); diff --git a/src/main/java/frc/robot/constants/AprilTag.java b/src/main/java/frc/robot/constants/AprilTag.java new file mode 100644 index 00000000..5ab07da9 --- /dev/null +++ b/src/main/java/frc/robot/constants/AprilTag.java @@ -0,0 +1,37 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.utils.PathPlannerUtils; +import frc.robot.utils.TrapPositionList; + +public class AprilTag { + private NetworkTableInstance inst; + private NetworkTable table; + private DoubleSubscriber subscriber; + + public AprilTag() { + inst = NetworkTableInstance.getDefault(); + table = inst.getTable("ROS"); + subscriber = table.getDoubleTopic("apriltag_id").subscribe(0); + } + public TrapPositionList getTag(){ + int tagId = (int) subscriber.get(0); + if (tagId == 0) { + return null; + } + return TrapPositionList.getTag((int)subscriber.getAsDouble()); + } + public Command planPath(){ + TrapPositionList tag = getTag(); + if (tag == null) { + return new InstantCommand(); + }return PathPlannerUtils.pathToPose(tag.getTrapPosition(), 0); + } + +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index e36f7319..a42f339f 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -7,6 +7,9 @@ public class GameConstants { public static final int XBOX_CONTROLLER_ID = 2; public static final double METERS_TO_INCHES = 39.37; + public static final double X_ADJUSTMENT = 17.625; + public static final double Y_ADJUSTMENT = 30.52; + public static final double FULL_ADJUMENT = 35.24; public static final double AUTO_SPOOL_AND_SHOOT_TIME = 4; //DEBUGS diff --git a/src/main/java/frc/robot/constants/TagConstants.java b/src/main/java/frc/robot/constants/TagConstants.java deleted file mode 100644 index f792afb1..00000000 --- a/src/main/java/frc/robot/constants/TagConstants.java +++ /dev/null @@ -1,79 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class TagConstants { - private static final double X_ADJUSTMENT = 10.625; - private static final double Y_ADJUSTMENT = 18.4; - private static final double FULL_ADJUMENT = 21.25; - private static TrapPositionList lowestTag; - private static NetworkTableInstance inst = NetworkTableInstance.getDefault(); - private static NetworkTable table = inst.getTable("ROS"); - private static DoubleSubscriber subscriber = table.getDoubleTopic("apriltag_id").subscribe(0); - - enum TrapPositionList { - ELEVEN(11, 468.660 + X_ADJUSTMENT, 146.063 - Y_ADJUSTMENT, 120), - TWELVE(12, 468.660 + X_ADJUSTMENT, 177.099 + Y_ADJUSTMENT, 240), - THIRTEEN(13, 441.739 - FULL_ADJUMENT, 161.620, 0), - FOURTEEN(14, 209.480 + FULL_ADJUMENT, 161.620, 180), - FIFTEEN(15, 182.72 - X_ADJUSTMENT, 177.099 + Y_ADJUSTMENT, 300), - SIXTEEN(16, 182.72 - X_ADJUSTMENT, 146.063 - Y_ADJUSTMENT, 60); - - private final int tagId; - private final double xPos; - private final double yPos; - private final double rot; - - - TrapPositionList(int tagId, double xPos, double yPos, double rot) { - this.tagId = tagId; - this.xPos = xPos; - this.yPos = yPos; - this.rot = rot; - } - public double getXPos() { return xPos;} - public double getYPos() { return yPos;} - public double getRot() { return rot;} - public double getTagID() {return tagId;} - } - - public static void setLowestTag() { - - if (subscriber.get() == 11) { - lowestTag = TrapPositionList.ELEVEN; - } - if (subscriber.get() == 12) { - lowestTag = TrapPositionList.TWELVE; - } - if (subscriber.get() == 13) { - lowestTag = TrapPositionList.THIRTEEN; - } - if (subscriber.get() == 14) { - lowestTag = TrapPositionList.FOURTEEN; - } - if (subscriber.get() == 15) { - lowestTag = TrapPositionList.FIFTEEN; - } - if (subscriber.get() == 16) { - lowestTag = TrapPositionList.SIXTEEN; - } else { - lowestTag = null; - } - - } - - public static Pose2d getTrapPosition() { - setLowestTag(); - if (lowestTag != null) { - return new Pose2d(lowestTag.getXPos() / Constants.METERS_TO_INCHES, lowestTag.getYPos() / Constants.METERS_TO_INCHES, new Rotation2d(lowestTag.getRot())); - } else { - return null; - } - } -} - - diff --git a/src/main/java/frc/robot/utils/TrapPositionList.java b/src/main/java/frc/robot/utils/TrapPositionList.java new file mode 100644 index 00000000..3b3c5a7b --- /dev/null +++ b/src/main/java/frc/robot/utils/TrapPositionList.java @@ -0,0 +1,52 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.constants.Constants; + +public enum TrapPositionList { + + ELEVEN(11, 468.660 + Constants.X_ADJUSTMENT, 146.063 - Constants.Y_ADJUSTMENT, 120), + TWELVE(12, 468.660 + Constants.X_ADJUSTMENT, 177.099 + Constants.Y_ADJUSTMENT, 240), + THIRTEEN(13, 441.739 - Constants.FULL_ADJUMENT, 161.620, 0), + FOURTEEN(14, 209.480 + Constants.FULL_ADJUMENT, 161.620, 180), + FIFTEEN(15, 182.72 - Constants.X_ADJUSTMENT, 177.099 + Constants.Y_ADJUSTMENT, 300), + SIXTEEN(16, 182.72 - Constants.X_ADJUSTMENT, 146.063 - Constants.Y_ADJUSTMENT, 240); + + private final int tagId; + private final double xPos; + private final double yPos; + private final double rot; + + TrapPositionList(int tagId, double xPos, double yPos, double rot) { + this.tagId = tagId; + this.xPos = xPos; + this.yPos = yPos; + this.rot = rot; + } + + public Pose2d getTrapPosition() { + return new Pose2d(getXPos() / Constants.METERS_TO_INCHES, getYPos() / Constants.METERS_TO_INCHES, + new Rotation2d(getRot())); + } + + public static TrapPositionList getTag(int tag) { + return TrapPositionList.values()[tag - 11]; + } + + public double getXPos() { + return xPos; + } + + public double getYPos() { + return yPos; + } + + public double getRot() { + return rot; + } + + public double getTagID() { + return tagId; + } + } From 547fd5c9b74e09957aaf1fbfb08dfe25c2be9145 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Sat, 23 Mar 2024 16:05:27 -0400 Subject: [PATCH 7/9] Radian Stuff --- .../java/frc/robot/constants/AprilTag.java | 18 ++++++++++-------- .../java/frc/robot/utils/TrapPositionList.java | 2 +- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/constants/AprilTag.java b/src/main/java/frc/robot/constants/AprilTag.java index 5ab07da9..6e72cf5b 100644 --- a/src/main/java/frc/robot/constants/AprilTag.java +++ b/src/main/java/frc/robot/constants/AprilTag.java @@ -1,7 +1,5 @@ package frc.robot.constants; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -20,18 +18,22 @@ public AprilTag() { table = inst.getTable("ROS"); subscriber = table.getDoubleTopic("apriltag_id").subscribe(0); } - public TrapPositionList getTag(){ - int tagId = (int) subscriber.get(0); + + public TrapPositionList getTag() { + double tagId = subscriber.get(0); if (tagId == 0) { return null; } - return TrapPositionList.getTag((int)subscriber.getAsDouble()); + return TrapPositionList.getTag((int) subscriber.get()); } - public Command planPath(){ + + public Command planPath() { TrapPositionList tag = getTag(); if (tag == null) { return new InstantCommand(); - }return PathPlannerUtils.pathToPose(tag.getTrapPosition(), 0); - } + } else { + return PathPlannerUtils.pathToPose(tag.getTrapPosition(), 0); + } + } } diff --git a/src/main/java/frc/robot/utils/TrapPositionList.java b/src/main/java/frc/robot/utils/TrapPositionList.java index 3b3c5a7b..2d62565c 100644 --- a/src/main/java/frc/robot/utils/TrapPositionList.java +++ b/src/main/java/frc/robot/utils/TrapPositionList.java @@ -27,7 +27,7 @@ public enum TrapPositionList { public Pose2d getTrapPosition() { return new Pose2d(getXPos() / Constants.METERS_TO_INCHES, getYPos() / Constants.METERS_TO_INCHES, - new Rotation2d(getRot())); + Rotation2d.fromDegrees(getRot())); } public static TrapPositionList getTag(int tag) { From 3c328a6b8e0351368bf71c3103ac523d49c0c80d Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Sat, 23 Mar 2024 16:17:16 -0400 Subject: [PATCH 8/9] Formatting --- .../frc/robot/utils/TrapPositionList.java | 87 +++++++++---------- 1 file changed, 43 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/utils/TrapPositionList.java b/src/main/java/frc/robot/utils/TrapPositionList.java index 2d62565c..2fecfead 100644 --- a/src/main/java/frc/robot/utils/TrapPositionList.java +++ b/src/main/java/frc/robot/utils/TrapPositionList.java @@ -5,48 +5,47 @@ import frc.robot.constants.Constants; public enum TrapPositionList { - - ELEVEN(11, 468.660 + Constants.X_ADJUSTMENT, 146.063 - Constants.Y_ADJUSTMENT, 120), - TWELVE(12, 468.660 + Constants.X_ADJUSTMENT, 177.099 + Constants.Y_ADJUSTMENT, 240), - THIRTEEN(13, 441.739 - Constants.FULL_ADJUMENT, 161.620, 0), - FOURTEEN(14, 209.480 + Constants.FULL_ADJUMENT, 161.620, 180), - FIFTEEN(15, 182.72 - Constants.X_ADJUSTMENT, 177.099 + Constants.Y_ADJUSTMENT, 300), - SIXTEEN(16, 182.72 - Constants.X_ADJUSTMENT, 146.063 - Constants.Y_ADJUSTMENT, 240); - - private final int tagId; - private final double xPos; - private final double yPos; - private final double rot; - - TrapPositionList(int tagId, double xPos, double yPos, double rot) { - this.tagId = tagId; - this.xPos = xPos; - this.yPos = yPos; - this.rot = rot; - } - - public Pose2d getTrapPosition() { - return new Pose2d(getXPos() / Constants.METERS_TO_INCHES, getYPos() / Constants.METERS_TO_INCHES, - Rotation2d.fromDegrees(getRot())); - } - - public static TrapPositionList getTag(int tag) { - return TrapPositionList.values()[tag - 11]; - } - - public double getXPos() { - return xPos; - } - - public double getYPos() { - return yPos; - } - - public double getRot() { - return rot; - } - - public double getTagID() { - return tagId; - } + + ELEVEN(11, 468.660 + Constants.X_ADJUSTMENT, 146.063 - Constants.Y_ADJUSTMENT, 120), + TWELVE(12, 468.660 + Constants.X_ADJUSTMENT, 177.099 + Constants.Y_ADJUSTMENT, 240), + THIRTEEN(13, 441.739 - Constants.FULL_ADJUMENT, 161.620, 0), + FOURTEEN(14, 209.480 + Constants.FULL_ADJUMENT, 161.620, 180), + FIFTEEN(15, 182.72 - Constants.X_ADJUSTMENT, 177.099 + Constants.Y_ADJUSTMENT, 300), + SIXTEEN(16, 182.72 - Constants.X_ADJUSTMENT, 146.063 - Constants.Y_ADJUSTMENT, 240); + + private final int tagId; + private final double xPos; + private final double yPos; + private final double rot; + + TrapPositionList(int tagId, double xPos, double yPos, double rot) { + this.tagId = tagId; + this.xPos = xPos; + this.yPos = yPos; + this.rot = rot; + } + + public Pose2d getTrapPosition() { + return new Pose2d(getXPos() / Constants.METERS_TO_INCHES, getYPos() / Constants.METERS_TO_INCHES, Rotation2d.fromDegrees(getRot())); + } + + public static TrapPositionList getTag(int tag) { + return TrapPositionList.values()[tag - 11]; + } + + public double getXPos() { + return xPos; + } + + public double getYPos() { + return yPos; + } + + public double getRot() { + return rot; + } + + public double getTagID() { + return tagId; } +} From a75a2aa2e4d81ef9e4b37eb50c2337cb7a020965 Mon Sep 17 00:00:00 2001 From: Sahiltheram Date: Sat, 23 Mar 2024 16:18:49 -0400 Subject: [PATCH 9/9] Fixes --- src/main/java/frc/robot/utils/TrapPositionList.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/TrapPositionList.java b/src/main/java/frc/robot/utils/TrapPositionList.java index 2fecfead..f097fbdc 100644 --- a/src/main/java/frc/robot/utils/TrapPositionList.java +++ b/src/main/java/frc/robot/utils/TrapPositionList.java @@ -11,7 +11,7 @@ public enum TrapPositionList { THIRTEEN(13, 441.739 - Constants.FULL_ADJUMENT, 161.620, 0), FOURTEEN(14, 209.480 + Constants.FULL_ADJUMENT, 161.620, 180), FIFTEEN(15, 182.72 - Constants.X_ADJUSTMENT, 177.099 + Constants.Y_ADJUSTMENT, 300), - SIXTEEN(16, 182.72 - Constants.X_ADJUSTMENT, 146.063 - Constants.Y_ADJUSTMENT, 240); + SIXTEEN(16, 182.72 - Constants.X_ADJUSTMENT, 146.063 - Constants.Y_ADJUSTMENT, 60); private final int tagId; private final double xPos;