diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 54bb595..d95a6a8 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -10,10 +10,11 @@ import java.util.function.DoubleSupplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; - +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.SynchronousInterrupt.WaitResult; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.TunerConstants.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -51,7 +52,7 @@ public class AprilTag3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double[] BotPose = new double[7]; + double[] BotPose = new double[6]; Optional alliance = DriverStation.getAlliance(); if(alliance.get() == DriverStation.Alliance.Red){ BotPose = limelight3.getBotPoseRed(); @@ -60,18 +61,24 @@ public class AprilTag3 extends Command { BotPose = limelight3.getBotPoseBlue(); } double botx = BotPose[0]; - double botz = BotPose[2]; + double boty = BotPose[1]; double tx = limelight3.getTx(); - double tagId = BotPose[7]; + double tagId = limelight3.getTId(); if(limelight3.getV() == true){ - if(tagId ==8){ + if(tagId ==10){ drivetrain.setControl(drive. withRotationalRate(tx/20). - withVelocityX(2-botx). - withVelocityY(2-botz)); + withVelocityX((botx-5.81)*2). + withVelocityY((boty-4)*4)); + } + else if(tagId ==7){ + drivetrain.setControl(drive. + withRotationalRate(tx/20). + withVelocityX(-(botx-2.21)*2). + withVelocityY(-(boty-4)*4)); } } - else{ + else{ drivetrain.setControl(drive. withRotationalRate(0). withVelocityX(0). diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java index 42d768a..27a9a37 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -27,13 +27,13 @@ public class Limelight3 extends SubsystemBase { return targetPose; } public double[] getBotPoseBlue(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue"); double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); return BotPose; } public double[] getBotPoseRed(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired"); double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); return BotPose; @@ -48,6 +48,11 @@ public class Limelight3 extends SubsystemBase { NetworkTableEntry ta = table.getEntry("ta"); return ta.getDouble(0.0); } + public double getTId(){ + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon"); + NetworkTableEntry tid = table.getEntry("tid"); + return tid.getDouble(0.0); + } public double getRx(){ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTableEntry rx = table.getEntry("rx");