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

limelight s'anligne test

Browse Source
This commit is contained in:
Samuel
2025-11-12 18:56:01 -05:00
parent f590d88c2d
commit 820d7de306
2 changed files with 42 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,6 +10,7 @@ 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.MathUtil;
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;
@@ -47,14 +48,37 @@ 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 a = limelight3.getTx(); double[] targetPose = limelight3.getTargetPose();
double b = 1.46 / Math.sqrt((limelight3.getTA()/100) / (Math.cos(90-limelight3.getTx()))); double[] BotPose = limelight3.getBotPose();
double z = targetPose[2];
double x = targetPose[0];
double Botyaw = BotPose[5];
double tx = limelight3.getTx();
if(limelight3.getV() == true){ if(limelight3.getV() == true){
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. drivetrain.setControl(drive.
withRotationalRate(a/20). withRotationalRate(tx/20).
withVelocityX(b/50). withVelocityX(MathUtil.clamp(-x/4, -1, 1)).
withVelocityY( 0)); withVelocityY(MathUtil.clamp(z/4, -1, 1)));
System.out.println(b/50); }
else if(Botyaw >-90 && Botyaw <=0){
drivetrain.setControl(drive.
withRotationalRate(tx/20).
withVelocityX(MathUtil.clamp(-x/4, -1, 1)).
withVelocityY(MathUtil.clamp(-z/4, -1, 1)));
}
else if(Botyaw >-180 && Botyaw <=-90){
drivetrain.setControl(drive.
withRotationalRate(tx/20).
withVelocityX(MathUtil.clamp(x/4, -1, 1)).
withVelocityY(MathUtil.clamp(-z/4, -1, 1)));
}
} }
else{ else{
drivetrain.setControl(drive. drivetrain.setControl(drive.

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

@@ -20,6 +20,18 @@ public class Limelight3 extends SubsystemBase {
PortForwarder.add(port, "limelight.local", port); PortForwarder.add(port, "limelight.local", port);
} }
} }
public double[] getTargetPose(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");
double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
return targetPose;
}
public double[] getBotPose(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_cameraspace");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[6]);
return BotPose;
}
public double getTx(){ public double getTx(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry tx = table.getEntry("tx"); NetworkTableEntry tx = table.getEntry("tx");