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

Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025

Browse Source
This commit is contained in:
Antoine PerreaultE
2025-11-17 18:17:19 -05:00
parent 3ec58dca1a 22271c8106
commit 55ff246629
2 changed files with 26 additions and 24 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

@@ -5,12 +5,15 @@
package frc.robot.commands.Limelight; package frc.robot.commands.Limelight;
import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.*;
import java.util.Optional;
import java.util.function.DoubleSupplier; 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.math.MathUtil;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
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;
@@ -48,31 +51,24 @@ 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[] targetPose = limelight3.getTargetPose(); double[] BotPose = new double[7];
double[] BotPose = limelight3.getBotPose(); Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
double z = targetPose[2]; if(alliance.get() == DriverStation.Alliance.Red){
double x = targetPose[0]; BotPose = limelight3.getBotPoseRed();
double Botyaw = BotPose[5]; }
else{
BotPose = limelight3.getBotPoseBlue();
}
double botx = BotPose[0];
double botz = BotPose[2];
double tx = limelight3.getTx(); double tx = limelight3.getTx();
double tagId = BotPose[7];
if(limelight3.getV() == true){ if(limelight3.getV() == true){
System.out.println(Botyaw); if(tagId ==8){
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){
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.

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

@@ -26,10 +26,16 @@ public class Limelight3 extends SubsystemBase {
double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]); double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
return targetPose; return targetPose;
} }
public double[] getBotPose(){ public double[] getBotPoseBlue(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose"); NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
double[] BotPose = BotPoseEntry.getDoubleArray(new double[6]); 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; return BotPose;
} }
public double getTx(){ public double getTx(){