m

diff --git a/src/main/java/frc/robot/commands/gpm/AimAtPose.java b/src/main/java/frc/robot/commands/gpm/AimAtPose.java
deleted file mode 100644 (file)
index 4b204d3..0000000
--- a/src/main/java/frc/robot/commands/gpm/AimAtPose.java
+++ /dev/null
@@ -1,63 +0,0 @@
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.apriltag.AprilTag;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.FieldConstants;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.subsystems.turret.Turret;
-
-/**
- * Aims the robot at the closest April tag
- */
-public class AimAtPose extends Command {
-  private Turret turret;
-  private Drivetrain drive;
-  private Pose2d target;
-
-  /**
-   * Aims the robot at the closest April tag
-   * @param drive The drivetrain
-   */
-  public AimAtPose(Drivetrain drive, Turret turret, Pose2d target){
-    this.turret = turret;
-    this.drive = drive;
-    this.target = target;
-    addRequirements(turret);
-  }
-
-  /**
-   * Gets the closest tag and sets the setpoint to aim at it
-   */
-  @Override
-  public void initialize(){}
-  
-  @Override
-  public void execute() {
-    //Logger.recordOutput("TurretPose", new Pose2d(drive.getPose().getTranslation(), turret.getPosition()));
-    //.setSetpoint(-target.getTranslation().minus(drive.getPose().getTranslation()).getAngle().getDegrees(), 0);
-  }
-
-  /**
-   * Stops the drivetrain
-   * @param interrupted If the command is interrupted
-   */
-  @Override
-  public void end(boolean interrupted) {
-    
-  }
-
-  /**
-   * Returns if the command is finished
-   * @return If the PID is at the setpoint
-   */
-  @Override
-  public boolean isFinished() {
-    // return turret.atSetPoint();
-    return false;
-  }
-}
-

diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
index bdb5f1ccd415e6bbd8b96fad664f530d4bb7409b..60dcbfe6f274ec3266a7a9f26fc5a6f3e425da3c 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
@@ -91,20 +91,6 @@ public class SimpleAutoShoot extends Command {
         // System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
     }
 
-    public void adjustWithTurretCam() {
-        if(turretVision.canSeeTag(26) || turretVision.canSeeTag(10)){
-            // Adjust turret setpoint based on vision input
-            if(Robot.getAlliance() == Alliance.Blue){
-                yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(26).get());
-            }
-            else{
-                yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(10).get());
-            }
-            double error = yawToTagCamera - yawToTag;
-            adjustedSetpoint = turretSetpoint + error;
-        }
-    }
-
     public void updateYawToTag(){
         // Calculate the yaw offset to the tag
         double D_y = FieldConstants.getHubTranslation().getY() - drivetrain.getPose().getY();