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

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

@@ -54,30 +54,42 @@ public class AprilTag3 extends Command {
public void execute() {
double[] BotPose = new double[6];
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){
BotPose = limelight3.getBotPoseRed();
x2 =11.9;
}
else{
BotPose = limelight3.getBotPoseBlue();
x2 = 4.1;
}
double botx = BotPose[0];
double boty = BotPose[1];
double tx = limelight3.getTx();
double tagId = limelight3.getTId();
if(limelight3.getV() == true){
if(tagId ==10){
drivetrain.setControl(drive.
withRotationalRate(tx/20).
withVelocityX((botx-5.81)*2).
withVelocityY((boty-4)*4));
}
else if(tagId ==7){
drivetrain.setControl(drive.
withRotationalRate(tx/20).
withVelocityX(2-botx).
withVelocityY(2-boty));
drivetrain.setControl(drive.
withRotationalRate(limelight3.Calcule(botx,x2,boty,y2,angle)).
withVelocityX(x.getAsDouble()).
withVelocityY(y.getAsDouble()));
// if(tagId ==10){
// drivetrain.setControl(drive.
// withRotationalRate(tx/20).
// withVelocityX((botx-5.81)*2).
// withVelocityY((boty-4)*4));
// }
// else if(tagId ==7){
// drivetrain.setControl(drive.
// withRotationalRate(tx/20).
// withVelocityX(2-botx).
// withVelocityY(2-boty));
}
// }
}
else{
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(){
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
public void periodic() {
// This method will be called once per scheduler run