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

limelight pls marche

Browse Source
This commit is contained in:
Olivier Dubois
2026-03-31 05:19:16 +02:00
parent 1240160ed7
commit 098f16665a
3 changed files with 97 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -53,46 +53,63 @@ public class Limelighter extends Command {
double[] BotPose = new double[6]; double[] BotPose = new double[6];
System.out.println("e"); System.out.println("e");
if (limelight3g.getV()) { if (limelight3g.getV()) {
BotPose = limelight3g.getBotPoseBlue(); // BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) { if (!alliance.isPresent()) {
return; return;
} }
if (alliance.get() == Alliance.Blue) { if (alliance.get() == Alliance.Blue) {
x = 4.6; x = 4.6;
BotPose = limelight3g.getBotPoseBlue();
} }
else { else {
x = 11.915394; x = 11.915394;
BotPose = limelight3g.getBotPoseRed();
} }
botx = BotPose[1]; botx = BotPose[1];
boty = BotPose[0]; boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
calcul = limelight3g.Calcule(botx, x, boty, 4, angle); calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul < -5 && calcul > -180){ if(angle > 180){
drivetrain.setControl( angle -= 360;
drive.withRotationalRate(0.5*(2*Math.PI)));
} }
else if(calcul > 5 && calcul < 180){ if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else{
drivetrain.setControl( drivetrain.setControl(
drive.withRotationalRate(0)); drive.withRotationalRate(0));
} }
else if(calcul > 5){
drivetrain.setControl( drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); drive.withRotationalRate(-0.5*(2*Math.PI)));
System.out.println(angle);
if (calcul < 0.2 && calcul > -0.2) {
drivetrain.setControl(drive.withRotationalRate(0));
} }
else if(calcul < -5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
// if(calcul < -5 && calcul > -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else if(calcul > 5 && calcul < 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul >= 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul <= -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else{
// drivetrain.setControl(
// drive.withRotationalRate(0));
// }
// drivetrain.setControl(
// drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
// System.out.println(angle);
// if (calcul < 0.2 && calcul > -0.2) {
// drivetrain.setControl(drive.withRotationalRate(0));
// }
} }
else{ else{
drivetrain.setControl(drive.withRotationalRate(0)); drivetrain.setControl(drive.withRotationalRate(0));

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

@@ -53,46 +53,63 @@ public class LimelighterAuto extends Command {
double[] BotPose = new double[6]; double[] BotPose = new double[6];
System.out.println("e"); System.out.println("e");
if (limelight3g.getV()) { if (limelight3g.getV()) {
// BotPose = limelight3g.getBotPoseBlue();
if (!alliance.isPresent()) { if (!alliance.isPresent()) {
return; return;
} }
if (alliance.get() == Alliance.Blue) { if (alliance.get() == Alliance.Blue) {
x = 4.6; x = 4.6;
BotPose = limelight3g.getBotPoseBlue();
} }
else { else {
x = 11.915394; x = 11.915394;
BotPose = limelight3g.getBotPoseRed();
} }
BotPose = limelight3g.getBotPoseBlue();
botx = BotPose[1]; botx = BotPose[1];
boty = BotPose[0]; boty = BotPose[0];
angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
calcul = limelight3g.Calcule(botx, x, boty, 4, angle); calcul = limelight3g.Calcule(botx, x, boty, 4, angle);
if(calcul < -5 && calcul > -180){ if(angle > 180){
drivetrain.setControl( angle -= 360;
drive.withRotationalRate(0.5*(2*Math.PI)));
} }
else if(calcul > 5 && calcul < 180){ if(calcul > -5 && calcul < 5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul >= 180){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
else if(calcul <= -180){
drivetrain.setControl(
drive.withRotationalRate(0.5*(2*Math.PI)));
}
else{
drivetrain.setControl( drivetrain.setControl(
drive.withRotationalRate(0)); drive.withRotationalRate(0));
} }
else if(calcul > 5){
drivetrain.setControl( drivetrain.setControl(
drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); drive.withRotationalRate(-0.5*(2*Math.PI)));
System.out.println(angle);
if (calcul < 0.2 && calcul > -0.2) {
drivetrain.setControl(drive.withRotationalRate(0));
} }
else if(calcul < -5){
drivetrain.setControl(
drive.withRotationalRate(-0.5*(2*Math.PI)));
}
// if(calcul < -5 && calcul > -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else if(calcul > 5 && calcul < 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul >= 180){
// drivetrain.setControl(
// drive.withRotationalRate(-0.5*(2*Math.PI)));
// }
// else if(calcul <= -180){
// drivetrain.setControl(
// drive.withRotationalRate(0.5*(2*Math.PI)));
// }
// else{
// drivetrain.setControl(
// drive.withRotationalRate(0));
// }
// drivetrain.setControl(
// drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
// System.out.println(angle);
// if (calcul < 0.2 && calcul > -0.2) {
// drivetrain.setControl(drive.withRotationalRate(0));
// }
} }
else{ else{
drivetrain.setControl(drive.withRotationalRate(0)); drivetrain.setControl(drive.withRotationalRate(0));
@@ -109,7 +126,6 @@ public class LimelighterAuto extends Command {
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public boolean isFinished() { public boolean isFinished() {
return calcul < 5 && calcul > -5; return calcul > -5 && calcul < 5;
} }
} }

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

@@ -52,21 +52,27 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
} }
public double Calcule(double x1, double x2, double y1, double y2, double angle) public double Calcule(double x1, double x2, double y1, double y2, double angle)
{ {
if(x1 > x2){ // if(x1 > x2){
// if(y1 > y2){
// return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle;
// }
// else{
// return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle;
// }
// }
// else{
// if(y1 > y2){
// return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle;
// }
// else{
// return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle;
// }
// }
if(y1 > y2){ if(y1 > y2){
return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle; return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
} }
else{ else{
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle; return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)-360 - angle;
}
}
else{
if(y1 > y2){
return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle;
}
else{
return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle;
}
} }
} }
@Override @Override