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
2026-01-19 18:29:04 -05:00
parent 5a8a499e05 9fa5624504
commit 3f546b61d6
5 changed files with 48 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@@ -56,5 +56,6 @@
"edu.wpi.first.math.proto.*", "edu.wpi.first.math.proto.*",
"edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.proto.*",
"edu.wpi.first.math.**.struct.*", "edu.wpi.first.math.**.struct.*",
] ],
"java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
} }

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

@@ -54,22 +54,30 @@ public class AprilTag3 extends Command {
public void execute() { public void execute() {
double[] BotPose = new double[6]; double[] BotPose = new double[6];
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance(); Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
double y2 = 4;
double x2 = 0;
double angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
if(angle >180){
angle =- angle * 2;
}
if(alliance.get() == DriverStation.Alliance.Red){ if(alliance.get() == DriverStation.Alliance.Red){
BotPose = limelight3.getBotPoseRed(); BotPose = limelight3.getBotPoseRed();
x2 =11.9;
} }
else{ else{
BotPose = limelight3.getBotPoseBlue(); BotPose = limelight3.getBotPoseBlue();
x2 = 4.1;
} }
double botx = BotPose[0]; double botx = BotPose[0];
System.out.println(botx); System.out.println(botx);
double boty = BotPose[1]; double boty = BotPose[1];
double tx = limelight3.getTx(); double tx = limelight3.getTx();
double tagId = limelight3.getTId(); double tagId = limelight3.getTId();
if(limelight3.getV() == true){
drivetrain.setControl(drive. drivetrain.setControl(drive.
withRotationalRate(tx/20). withRotationalRate(limelight3.Calcule(botx,x2,boty,y2,angle)).
withVelocityX((botx-5.81)*2). withVelocityX(x.getAsDouble()).
withVelocityY((boty-4)*4)); withVelocityY(y.getAsDouble()));
// if(limelight3.getV() == true){
// if(tagId ==10){ // if(tagId ==10){
// drivetrain.setControl(drive. // drivetrain.setControl(drive.
// withRotationalRate(tx/20). // withRotationalRate(tx/20).
@@ -83,19 +91,13 @@ public class AprilTag3 extends Command {
// withVelocityY(2-boty)); // withVelocityY(2-boty));
// } // }
// } }
// if(limelight3.getV()){ else{
// drivetrain.setControl(drive. drivetrain.setControl(drive.
// withRotationalRate(limelight3.getTx()/10). withRotationalRate(0).
// withVelocityX(x.getAsDouble()). withVelocityX(0).
// withVelocityY(y.getAsDouble())); withVelocityY(0));
// } }
// else{
// drivetrain.setControl(drive.
// withRotationalRate(0).
// withVelocityX(0).
// withVelocityY(0));
// }
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.

2
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View File

@@ -48,7 +48,7 @@ Translation2d backRightLocation = new Translation2d(-0.3, -0.3);
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
private void configureAutoBuilder() { private void configureAutoBuilder() {
try { try {
var config = RobotConfig.fromGUISettings(); RobotConfig config = RobotConfig.fromGUISettings();
AutoBuilder.configure( AutoBuilder.configure(
() -> getState().Pose, // Supplier of current robot pose () -> getState().Pose, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto this::resetPose, // Consumer for seeding pose against auto

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

@@ -69,6 +69,17 @@ public class Limelight3 extends SubsystemBase {
public void Forme(){ public void Forme(){
pipeline.setNumber(0); pipeline.setNumber(0);
} }
public double Calcule(double x1, double x2, double y1, double y2, double angle)
{
if (x1 > 4)
{
return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle)/90;
}
else
{
return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1/90;
}
}
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run // This method will be called once per scheduler run