Side Angle Shooting (TurnToTarget)

2024-Robot/src/main/java/frc/robot/Constants.java

@@ -12,6 +12,7 @@ public final class Constants {
   public static final class Physical {
     public static final double FIELD_WIDTH = 8.2;
     public static final double FIELD_LENGTH = 16.63;
+    public static final double SPEAKER_DEPTH = inchesToMeters(18.11);
     public static final double ROBOT_RADIUS = inchesToMeters(15.429);
     public static final double WHEEL_DIAMETER = inchesToMeters(4);
     public static final double WHEEL_CIRCUMFERENCE = Math.PI * WHEEL_DIAMETER;

2024-Robot/src/main/java/frc/robot/commands/TurnToTargetAdjusted.java

@@ -0,0 +1,98 @@
+package frc.robot.commands;
+
+import java.util.ArrayList;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.Drive;
+import frc.robot.subsystems.Peripherals;
+import frc.robot.tools.controlloops.PID;
+
+public class TurnToTargetAdjusted extends Command {
+  private Drive drive;
+  private Peripherals peripherals;
+
+  private PID turnPID;
+  private double kP = 0.05;
+  private double kI = 0.0;
+  private double kD = 0.09;
+
+  private double speakerAngleDegrees;
+  private double targetPigeonAngleDegrees;
+  private double speakerElevationDegrees;
+
+  private double initTime;
+  private double timeout = 1.5;
+
+  private double robotAngleAllowedErrorDegrees = 2;
+
+  public TurnToTargetAdjusted(Drive drive, Peripherals peripherals) {
+    this.drive = drive;
+    this.peripherals = peripherals;
+    addRequirements(this.drive);
+  }
+
+  @Override
+  public void initialize() {
+    this.initTime = Timer.getFPGATimestamp();
+    this.turnPID = new PID(this.kP, this.kI, this.kD);
+    this.turnPID.setMinOutput(-2);
+    this.turnPID.setMaxOutput(2);
+    this.speakerAngleDegrees = 0;
+    this.speakerElevationDegrees = 1;
+  }
+
+  @Override
+  public void execute() {
+    double pigeonAngleDegrees = this.peripherals.getPigeonAngle();
+
+    ArrayList<Integer> ids = this.peripherals.getFrontCamIDs();
+
+    boolean canSeeTag = false;
+    for (int id : ids){
+      // System.out.println("ID: " + id);
+      if (id == 7 || id == 4){
+        canSeeTag = true;
+      }
+    }
+
+    double prevSpeakerAngleDegrees = this.speakerAngleDegrees;
+    this.speakerAngleDegrees = this.peripherals.getFrontCamTargetTx();
+    if (canSeeTag && this.speakerAngleDegrees < 90 && Math.abs(this.speakerAngleDegrees - prevSpeakerAngleDegrees) > 0.01){
+      this.targetPigeonAngleDegrees = pigeonAngleDegrees - this.speakerAngleDegrees;
+    }
+    this.speakerElevationDegrees = this.peripherals.getFrontCamTargetTy();
+    this.turnPID.setSetPoint(targetPigeonAngleDegrees + peripherals.getAddedTheta(targetPigeonAngleDegrees,  Constants.SetPoints.getDistFromAngle(this.speakerElevationDegrees), Constants.Physical.SPEAKER_DEPTH/2));
+    this.turnPID.updatePID(pigeonAngleDegrees);
+    double turnResult = -this.turnPID.getResult();
+
+    if (Math.abs(this.targetPigeonAngleDegrees - pigeonAngleDegrees) < this.robotAngleAllowedErrorDegrees){
+      this.drive.driveAutoAligned(0);
+    } else {
+      this.drive.driveAutoAligned(turnResult);
+    }
+
+    // System.out.println("Speaker Ang Deg: " + this.speakerAngleDegrees);
+    // System.out.println("Pigeon Angle: " + pigeonAngleDegrees);
+    // System.out.println("Targ Angle: " + this.targetPigeonAngleDegrees);
+    // System.out.println("Turn Result: " + turnResult);
+    // System.out.println("Can See Tag: " + canSeeTag);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    this.drive.autoRobotCentricTurn(0);
+  }
+
+  @Override
+  public boolean isFinished() {
+    // if (Timer.getFPGATimestamp() - this.initTime > this.timeout){
+    //   return true;
+    // } else if (Math.abs(this.speakerAngleDegrees) <= this.robotAngleAllowedErrorDegrees && this.canSeeTag){
+    //   return true;
+    // } else {
+      return false;
+    // }
+  }
+}

2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java

@@ -103,6 +103,39 @@ public ArrayList<Integer> getFrontCamIDs(){
     return ids;
   }
 
+    public double getAddedTheta(double targetAngleDegrees, double distanceMeters, double depthAddedMeters) {
+    targetAngleDegrees = standardizeAngleDegrees(targetAngleDegrees);
+    double targetAngleRadians = Math.toRadians(targetAngleDegrees);
+    targetAngleRadians -= (Math.PI)/2;
+    double addedAngleRadians;
+    if(targetAngleRadians >= Math.PI/2) {
+      targetAngleRadians -= Math.PI;
+      addedAngleRadians = (targetAngleRadians - Math.atan(Math.tan(targetAngleRadians) - depthAddedMeters/(distanceMeters*Math.cos(targetAngleRadians))));
+    } else {
+      addedAngleRadians = -(targetAngleRadians - Math.atan(Math.tan(targetAngleRadians) - depthAddedMeters/(distanceMeters*Math.cos(targetAngleRadians))));
+    }
+    return Math.toDegrees(addedAngleRadians);
+  }
+
+  public double standardizeAngleDegrees(double angleDegrees) {
+    if(angleDegrees >= 0 && angleDegrees < 360) {
+      return angleDegrees;
+    } else if(angleDegrees < 0) {
+      while(angleDegrees < 0) {
+        angleDegrees += 360;
+      }
+      return angleDegrees;
+    } else if(angleDegrees >= 360) {
+      while(angleDegrees >= 360) {
+        angleDegrees -= 360;
+      }
+      return angleDegrees;
+    } else {
+      System.out.println("Error Line 134 of Peripherals.java: Idk how this happened");
+      return angleDegrees;
+    }
+  }
+
   public JSONObject getFrontCamLatencies(){
     JSONObject latencies = new JSONObject();
     latencies.put("tl", this.frontCamTl.getDouble(0) / 1000);