sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

limelight

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-03-30 17:53:54 -04:00
parent f7e88619e9
commit 6afc342006
3 changed files with 32 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/main/java/frc/robot/commands/Limelighter.java
View File

@@ -27,6 +27,7 @@ public class Limelighter extends Command {
double boty;
double angle;
double calcul;
double x;
Optional<Alliance> alliance = DriverStation.getAlliance();
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
@@ -51,26 +52,34 @@ public class Limelighter extends Command {
System.out.println("e");
if (limelight3g.getV()) {
BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) {
return;
}
if (alliance.get() == Alliance.Blue) {
x = 4.6;
}
else {
x = 11.915394;
}
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle);
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul < -5 && calcul > -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else if(calcul > 5 && calcul < 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else{
drivetrain.setControl(

21
src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java
View File

@@ -27,6 +27,7 @@ public class LimelighterAuto extends Command {
double boty;
double angle;
double calcul;
double x;
Optional<Alliance> alliance = DriverStation.getAlliance();
private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
.withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
@@ -50,27 +51,35 @@ public class LimelighterAuto extends Command {
double[] BotPose = new double[6];
System.out.println("e");
if (limelight3g.getV()) {
if (!alliance.isPresent()) {
return;
}
if (alliance.get() == Alliance.Blue) {
x = 4.6;
}
else {
x = 11.915394;
}
BotPose = limelight3g.getBotPoseBlue();
botx = BotPose[1];
boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle);
calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul < -5 && calcul > -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else if(calcul > 5 && calcul < 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*180/Math.PI));
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*180/Math.PI));
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else{
drivetrain.setControl(

4
src/main/java/frc/robot/subsystems/Limelight3G.java
View File

@@ -62,10 +62,10 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
}
else{
if(y1 > y2){
return Math.atan(90-((x2 - x1) / (y2 - y1)))* (180 / Math.PI)+270 - angle;
return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle;
}
else{
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+180 - angle;
return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle;
}
}
}