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

Browse Source
This commit is contained in:
Antoine PerreaultE
2025-11-17 18:14:43 -05:00
parent 04718b8400
commit 3ec58dca1a
2 changed files with 9 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,23 +55,24 @@ public class AprilTag3 extends Command {
double Botyaw = BotPose[5]; double Botyaw = BotPose[5];
double tx = limelight3.getTx(); double tx = limelight3.getTx();
if(limelight3.getV() == true){ if(limelight3.getV() == true){
System.out.println(Botyaw);
if(Botyaw > 0 && Botyaw <=90){ if(Botyaw > 0 && Botyaw <=90){
drivetrain.setControl(drive. drivetrain.setControl(drive.
withRotationalRate(tx/20). withRotationalRate(tx/20).
withVelocityX(MathUtil.clamp(x/4, -1, 1)). withVelocityX(MathUtil.clamp(-x/4, -1, 1)).
withVelocityY(MathUtil.clamp(z/4, -1, 1))); withVelocityY(MathUtil.clamp(z/4, -1, 1)));
} }
else if(Botyaw >90 && Botyaw <=180){ else if(Botyaw >90 && Botyaw <=180){
drivetrain.setControl(drive. drivetrain.setControl(drive.
withRotationalRate(tx/20). withRotationalRate(0).
withVelocityX(MathUtil.clamp(-x/4, -1, 1)). withVelocityX(MathUtil.clamp(x/4, -1, 1)).
withVelocityY(MathUtil.clamp(z/4, -1, 1))); withVelocityY(MathUtil.clamp(-z/4, -1, 1)));
} }
else if(Botyaw >-90 && Botyaw <=0){ else if(Botyaw >-90 && Botyaw <=0){
drivetrain.setControl(drive. drivetrain.setControl(drive.
withRotationalRate(tx/20). withRotationalRate(tx/20).
withVelocityX(MathUtil.clamp(-x/4, -1, 1)). 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){ else if(Botyaw >-180 && Botyaw <=-90){
drivetrain.setControl(drive. drivetrain.setControl(drive.

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

@@ -21,14 +21,14 @@ public class Limelight3 extends SubsystemBase {
} }
} }
public double[] getTargetPose(){ public double[] getTargetPose(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace"); NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");
double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]); double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
return targetPose; return targetPose;
} }
public double[] getBotPose(){ public double[] getBotPose(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_cameraspace"); NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[6]); double[] BotPose = BotPoseEntry.getDoubleArray(new double[6]);
return BotPose; return BotPose;
} }