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

s'enligner sur sible fixe avec limelight

Browse Source
This commit is contained in:
Samuel
2026-01-15 14:20:13 -05:00
parent 2b5a298433
commit 9fa5624504
3 changed files with 37 additions and 13 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"
} }

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

@@ -54,30 +54,42 @@ 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];
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){ if(limelight3.getV() == true){
if(tagId ==10){
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(tagId ==10){
else if(tagId ==7){ // drivetrain.setControl(drive.
drivetrain.setControl(drive. // withRotationalRate(tx/20).
withRotationalRate(tx/20). // withVelocityX((botx-5.81)*2).
withVelocityX(2-botx). // withVelocityY((boty-4)*4));
withVelocityY(2-boty)); // }
// else if(tagId ==7){
// drivetrain.setControl(drive.
// withRotationalRate(tx/20).
// withVelocityX(2-botx).
// withVelocityY(2-boty));
} // }
} }
else{ else{
drivetrain.setControl(drive. drivetrain.setControl(drive.

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