MoveToTrap Code

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,13 +51,15 @@
 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;
 import frc.robot.swervev2.SwervePidConfig;
 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)));
 

src/main/java/frc/robot/commands/shooter/ShootAmp.java → 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);
         }
     }

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);
+        }
+
+    }
+}

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;

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

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;
+    }
+}