diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index df05ca0..2eead71 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -5,12 +5,15 @@ package frc.robot.commands.Limelight; import static edu.wpi.first.units.Units.*; +import java.util.Optional; import java.util.function.DoubleSupplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.TunerConstants.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -48,31 +51,24 @@ public class AprilTag3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double[] targetPose = limelight3.getTargetPose(); - double[] BotPose = limelight3.getBotPose(); - double z = targetPose[2]; - double x = targetPose[0]; - double Botyaw = BotPose[5]; + double[] BotPose = new double[7]; + Optional alliance = DriverStation.getAlliance(); + if(alliance.get() == DriverStation.Alliance.Red){ + BotPose = limelight3.getBotPoseRed(); + } + else{ + BotPose = limelight3.getBotPoseBlue(); + } + double botx = BotPose[0]; + double botz = BotPose[2]; double tx = limelight3.getTx(); + double tagId = BotPose[7]; if(limelight3.getV() == true){ - System.out.println(Botyaw); - if(Botyaw > 0 && Botyaw <=90){ - drivetrain.setControl(drive. - withRotationalRate(tx/20). - withVelocityX(MathUtil.clamp(-x/4, -1, 1)). - withVelocityY(MathUtil.clamp(z/4, -1, 1))); - } - else if(Botyaw >90 && Botyaw <=180){ - drivetrain.setControl(drive. - withRotationalRate(0). - withVelocityX(MathUtil.clamp(x/4, -1, 1)). - withVelocityY(MathUtil.clamp(-z/4, -1, 1))); - } - else if(Botyaw >-90 && Botyaw <=0){ + if(tagId ==8){ drivetrain.setControl(drive. withRotationalRate(tx/20). withVelocityX(MathUtil.clamp(-x/4, -1, 1)). - withVelocityY(MathUtil.clamp(z/4, -1, 1))); + withVelocityY(MathUtil.clamp(-z/4, -1, 1))); } else if(Botyaw >-180 && Botyaw <=-90){ drivetrain.setControl(drive. diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java index d86abc0..42d768a 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -26,10 +26,16 @@ public class Limelight3 extends SubsystemBase { double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]); return targetPose; } - public double[] getBotPose(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose"); - double[] BotPose = BotPoseEntry.getDoubleArray(new double[6]); + public double[] getBotPoseBlue(){ + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue"); + double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); + return BotPose; + } + public double[] getBotPoseRed(){ + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired"); + double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); return BotPose; } public double getTx(){