PLS5618/Reefscape-2025
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

s'enligne vers le apriltag

Browse Source
This commit is contained in:
Antoine PerreaultE
2025-11-24 17:35:46 -05:00
parent 2b6839b686
commit 6f32b4424d
2 changed files with 22 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/main/java/frc/robot/commands/Limelight/AprilTag3.java
View File

@@ -10,10 +10,11 @@ import java.util.function.DoubleSupplier;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.SynchronousInterrupt.WaitResult;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.TunerConstants.TunerConstants; import frc.robot.TunerConstants.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain; 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. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {
double[] BotPose = new double[7]; double[] BotPose = new double[6];
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance(); Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
if(alliance.get() == DriverStation.Alliance.Red){ if(alliance.get() == DriverStation.Alliance.Red){
BotPose = limelight3.getBotPoseRed(); BotPose = limelight3.getBotPoseRed();
@@ -60,18 +61,24 @@ public class AprilTag3 extends Command {
BotPose = limelight3.getBotPoseBlue(); BotPose = limelight3.getBotPoseBlue();
} }
double botx = BotPose[0]; double botx = BotPose[0];
double botz = BotPose[2]; double boty = BotPose[1];
double tx = limelight3.getTx(); double tx = limelight3.getTx();
double tagId = BotPose[7]; double tagId = limelight3.getTId();
if(limelight3.getV() == true){ if(limelight3.getV() == true){
if(tagId ==8){ if(tagId ==10){
drivetrain.setControl(drive. drivetrain.setControl(drive.
withRotationalRate(tx/20). withRotationalRate(tx/20).
withVelocityX(2-botx). withVelocityX((botx-5.81)*2).
withVelocityY(2-botz)); 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. drivetrain.setControl(drive.
withRotationalRate(0). withRotationalRate(0).
withVelocityX(0). withVelocityX(0).

9
src/main/java/frc/robot/subsystems/Limelight3.java
View File

@@ -27,13 +27,13 @@ public class Limelight3 extends SubsystemBase {
return targetPose; return targetPose;
} }
public double[] getBotPoseBlue(){ public double[] getBotPoseBlue(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue"); NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose; return BotPose;
} }
public double[] getBotPoseRed(){ public double[] getBotPoseRed(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired"); NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]); double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
return BotPose; return BotPose;
@@ -48,6 +48,11 @@ public class Limelight3 extends SubsystemBase {
NetworkTableEntry ta = table.getEntry("ta"); NetworkTableEntry ta = table.getEntry("ta");
return ta.getDouble(0.0); 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(){ public double getRx(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry rx = table.getEntry("rx"); NetworkTableEntry rx = table.getEntry("rx");