diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f89f23a..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,6 +51,7 @@ import frc.robot.commands.shooter.StopShooter; import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; +import frc.robot.constants.AprilTag; import frc.robot.subsystems.*; import frc.robot.swervev2.KinematicsConversionConfig; import frc.robot.swervev2.SwerveIdConfig; @@ -56,6 +59,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; @@ -91,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. @@ -233,6 +238,11 @@ private void configureBindings() { new RampMove(ramp, () -> GameConstants.RAMP_POS_SHOOT_SPEAKER_AWAY), new ShootSpeaker(shooter, drivetrain, lightStrip))); + // Set up to shoot TRAP - A + controller.a().onTrue(new InstantCommand(()->aprilTag.planPath().schedule())); + // 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/AprilTag.java b/src/main/java/frc/robot/constants/AprilTag.java new file mode 100644 index 00000000..6e72cf5b --- /dev/null +++ b/src/main/java/frc/robot/constants/AprilTag.java @@ -0,0 +1,39 @@ +package frc.robot.constants; + +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() { + double tagId = subscriber.get(0); + if (tagId == 0) { + return null; + } + return TrapPositionList.getTag((int) subscriber.get()); + } + + public Command planPath() { + TrapPositionList tag = getTag(); + if (tag == null) { + return new InstantCommand(); + } else { + 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 c02c55f5..a42f339f 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -6,6 +6,11 @@ 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 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 public static final boolean SHOOTER_DEBUG = false; @@ -50,7 +55,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; @@ -62,7 +67,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; @@ -80,7 +85,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 b22a87f2..5b52d954 100644 --- a/src/main/java/frc/robot/subsystems/Ramp.java +++ b/src/main/java/frc/robot/subsystems/Ramp.java @@ -108,7 +108,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 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..f097fbdc --- /dev/null +++ b/src/main/java/frc/robot/utils/TrapPositionList.java @@ -0,0 +1,51 @@ +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, 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 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; + } +}