led selon distance

src/main/java/frc/robot/commands/DistanceLancer.java

@@ -9,11 +9,11 @@ import frc.robot.subsystems.Led;
 import frc.robot.subsystems.Limelight3G;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
-public class DitanceLancer extends Command {
+public class DistanceLancer extends Command {
   private Led led;
   private Limelight3G limelight3g;
   /** Creates a new DitanceLancer. */
-  public DitanceLancer(Limelight3G limelight3g, Led led) {
+  public DistanceLancer(Limelight3G limelight3g, Led led) {
     this.led = led;
     this.limelight3g = limelight3g;
     addRequirements(led,limelight3g);
@@ -27,12 +27,31 @@ public class DitanceLancer extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    
+    double botx = 0;
+    double boty = 0;
+    double[] BotPose = new double[6];
+    double distance = 0;
+    if(limelight3g.getV()){
+      BotPose = limelight3g.getBotPoseBlue();
+      botx = BotPose[0];
+      boty = BotPose[1];
+    }                                        
+    if(limelight3g.getV()){
+      distance = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2));
+    }
+    if(distance > 1.7){
+      led.Jaune2();
+    }
+    else{
+      
+    }
   }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+
+  }
 
   // Returns true when the command should end.
   @Override