From 77b5d4c1b2ade085ff904b0df2e6f04658a67052 Mon Sep 17 00:00:00 2001 From: Ben-Mick Date: Sat, 2 Mar 2024 14:40:59 -0700 Subject: [PATCH] initial commit --- .../src/main/java/frc/robot/Constants.java | 1 + .../robot/commands/TurnToTargetAdjusted.java | 98 +++++++++++++++++++ .../frc/robot/subsystems/Peripherals.java | 33 +++++++ 3 files changed, 132 insertions(+) create mode 100644 2024-Robot/src/main/java/frc/robot/commands/TurnToTargetAdjusted.java diff --git a/2024-Robot/src/main/java/frc/robot/Constants.java b/2024-Robot/src/main/java/frc/robot/Constants.java index c9d21924..c5dca6bb 100644 --- a/2024-Robot/src/main/java/frc/robot/Constants.java +++ b/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; diff --git a/2024-Robot/src/main/java/frc/robot/commands/TurnToTargetAdjusted.java b/2024-Robot/src/main/java/frc/robot/commands/TurnToTargetAdjusted.java new file mode 100644 index 00000000..b534dc95 --- /dev/null +++ b/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 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; + // } + } +} diff --git a/2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java b/2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java index b1820a4f..6726266f 100644 --- a/2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java +++ b/2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java @@ -103,6 +103,39 @@ public ArrayList 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);