debug de samedi

src/main/java/frc/robot/Robot.java

@@ -4,9 +4,11 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.Limelight3G;
 
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
@@ -20,6 +22,15 @@ public class Robot extends TimedRobot {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+    var driveState = m_robotContainer.drivetrain.getState();
+    double headingDeg = driveState.Pose.getRotation().getDegrees();
+    double omegaRps = Units.radiansToRotations(driveState.Speeds.omegaRadiansPerSecond);
+
+    LimelightHelpers.SetRobotOrientation("limelight_tag", headingDeg, 0, 0, 0, 0, 0);
+    var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight_tag");
+    if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) {
+        m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds);
+    }
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -25,6 +25,7 @@ import frc.robot.commands.LancerAspirer;
 import frc.robot.commands.LancerBaseVitesse;
 import frc.robot.commands.Limelighter;
 import frc.robot.commands.ModeOposer;
+import frc.robot.commands.ModeOposerBalayeuse;
 import frc.robot.commands.ModeOposerDemeleur;
 import frc.robot.commands.MonterBalyeuse;
 import frc.robot.commands.MonterGrimpeur;
@@ -55,7 +56,7 @@ public class RobotContainer {
   Led led = new Led();
   CommandXboxController manette = new CommandXboxController(0);
   CommandXboxController manette1 = new CommandXboxController(1);
-  private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
     private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
 
     /* Setting up bindings for necessary control of the swerve drive platform */
@@ -97,25 +98,31 @@ public class RobotContainer {
         )
       );
     //manette 1
-    manette.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    // manette1.povUp().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    manette.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    // manette1.povDown().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-    manette.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    // manette1.povRight().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    manette.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    // manette1.povLeft().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
     
-    manette.povUp().whileTrue(new LancerAuto(lanceur));
+    manette.povUp().whileTrue(new MonterGrimpeur(grimpeur));
-    manette.a().whileTrue(new Lancer(lanceur,limeLight3G,balayeuse,led));
+    manette.povDown().whileTrue(new DescendreGrimpeur(grimpeur));
-    manette.x().whileTrue(new LancerBaseVitesse(lanceur, limeLight3));
+    manette.rightTrigger().whileTrue(new Lancer(lanceur, limeLight3G));
-    manette.rightTrigger().whileTrue(new MonterGrimpeur(grimpeur));
+    manette.leftTrigger().whileTrue(new Limelighter(limeLight3G, drivetrain));
-    manette.leftTrigger().whileTrue(new DescendreGrimpeur(grimpeur));
-    manette.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
     manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
+    manette.rightBumper().whileTrue(new Aspirer(balayeuse));
+    manette.y().whileTrue(new TournerAZero(drivetrain));
+    manette.b().whileTrue(new GrimperReservoir(limeLight3G, drivetrain));
+    manette.x().whileTrue(new GrimperMur(limeLight3, drivetrain));
+
 
-    manette.b().whileTrue(new Aspirer(balayeuse,led));
     //manette 2
-
+    manette1.rightTrigger().whileTrue(new Aspirer(balayeuse));
-    manette1.povDown().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3, led));
+    manette1.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));
-    manette1.y().whileTrue(new ModeOposer(lanceur, balayeuse));
+    manette1.leftTrigger().whileTrue(new LancerBaseVitesse(lanceur, limeLight3G));
-    manette1.x().whileTrue(new ModeOposerDemeleur(lanceur));
+    manette1.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse));
+    manette1.x().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3G));
+    manette1.b().whileTrue(new ModeOposer(lanceur));
+    manette1.a().whileTrue(new ModeOposerDemeleur(lanceur));
+    manette1.y().whileTrue(new ModeOposerBalayeuse(balayeuse));
   }
 
   public Command getAutonomousCommand() {

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

@@ -12,58 +12,30 @@ import frc.robot.subsystems.Led;
 /* 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 Aspirer extends Command {
   private Balayeuse balayeuse;
-  private Timer timer;
-  private Led led;
-  private double temp;
   /** Creates a new Aspirer. */
-  public Aspirer(Balayeuse balayeuse, Led led) {
+  public Aspirer(Balayeuse balayeuse) {
     this.balayeuse = balayeuse;
-    this.led = led;
+    addRequirements(balayeuse); 
-    this.timer = new Timer();
-    addRequirements(balayeuse, led); 
-    this.temp = 0;
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    timer.reset();
-    temp = balayeuse.Amp();  
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    timer.start();
+    balayeuse.BalayerEnbas(-0.5);
-    int nbFois = 0;
+    balayeuse.BalayerPadle(-0.2);
-    double moyenneAmp = 0;    
-    if(timer.get() <3){
-      nbFois++;
-      moyenneAmp += balayeuse.Amp() / nbFois;
-    }
-    else{
-      nbFois++;
-      moyenneAmp -= temp;
-      moyenneAmp += balayeuse.Amp() / nbFois;
-      temp = balayeuse.Amp();
-    }
-    if(moyenneAmp < balayeuse.AmpMax()){
-      timer.reset();
-      balayeuse.Balayer(-0.5);
-    }
-    else{
-        balayeuse.Balayer(0);
-        led.Jaune2();
-    }
-
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    balayeuse.Balayer(0);
+    balayeuse.BalayerEnbas(0);
-    timer.stop();
+    balayeuse.BalayerPadle(0);
   }
 
   // Returns true when the command should end.

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

@@ -25,7 +25,7 @@ public class DescendreBalyeuse extends Command {
   @Override
   public void execute() {
     if(Math.abs(balayeuse.Distance()) < balayeuse.EncodeurBalayeuse()){
-      balayeuse.Pivoter(-0.2);
+      balayeuse.Pivoter(-0.5);
     }
     else{
       balayeuse.Pivoter(0);

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

@@ -24,7 +24,7 @@ public class DescendreGrimpeur extends Command {
   @Override
   public void execute() {
     if(!grimpeur.Limit()){
-      grimpeur.Grimper(-0.3);
+      grimpeur.Grimper(-0.4);
     }
     else{
       grimpeur.Reset();

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

@@ -18,17 +18,13 @@ public class Lancer extends Command {
   private PIDController pidController;
   private Limelight3G limeLight3G;
   private Timer timer;
-  private Balayeuse balayeuse;
-  private Led led;
   private double temp;
   /** Creates a new Lancer. */
-  public Lancer(Lanceur lanceur, Limelight3G limeLight3G, Balayeuse balayeuse,Led led) {
+  public Lancer(Lanceur lanceur, Limelight3G limeLight3G) {
     this.lanceur = lanceur;
-    this.balayeuse = balayeuse;
-    this.led = led;
     this.timer = new Timer();
     this.limeLight3G = new Limelight3G();
-    addRequirements(lanceur, balayeuse, led, limeLight3G);
+    addRequirements(lanceur, limeLight3G);
     this.temp = 0;
     // Use addRequirements() here to declare subsystem dependencies.
   }
@@ -36,9 +32,8 @@ public class Lancer extends Command {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pidController = new PIDController(1, 0,0, 0);
+    pidController = new PIDController(0.0007, 0,0, 0.001);
     timer.reset();
-    timer.start();
     temp = lanceur.Amp();    
   }
 
@@ -53,39 +48,21 @@ public class Lancer extends Command {
       botx = BotPose[0];
       boty = BotPose[1];
     }                                        
-    int nbFois = 0;
-
-    double moyenneAmp = 0;
-    if(timer.get() < 3){
-      nbFois++;
-      moyenneAmp += balayeuse.Amp() / nbFois;
-    }
-    else{
-      nbFois++;
-      moyenneAmp -= temp;
-      moyenneAmp += balayeuse.Amp() / nbFois;
-      temp = balayeuse.Amp();
-    }
-    // if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){
-      timer.reset();
-      balayeuse.Balayer(-0.5);
-      // led.Jaune2();
-    // }
-    // else{
-                                                
          double vitesse = 0.5;
          if(limeLight3G.getV()){
-                                          //pythagore |
+            vitesse = 3100 + 475 * ((Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2))) - 2);
-                                          //         \/
+            System.out.println(vitesse);
-            vitesse = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2));
-         }                                   
-      double output = vitesse; /*pidController.calculate(lanceur.Vitesse(),vitesse);*/
-      lanceur.Lancer(output);
-      if(lanceur.Vitesse() >= vitesse){
-        lanceur.Demeler(0.1);
-      }
-    // }
 
+      double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+      lanceur.Lancer(output);
+      if(lanceur.Vitesse() >= vitesse-800){
+        timer.start();
+        if(timer.get() >0.5){
+          lanceur.Demeler(1);
+        }
+
+    }
+  }
   }
 
   // Called once the command ends or is interrupted.
@@ -93,7 +70,8 @@ public class Lancer extends Command {
   public void end(boolean interrupted) {
     lanceur.Demeler(0);
     lanceur.Lancer(0);
-    balayeuse.Pivoter(0);
+    timer.reset();
+    timer.stop();
   }
 
   // Returns true when the command should end.

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

@@ -17,8 +17,8 @@ import frc.robot.subsystems.Limelight3G;
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class LancerAspirer extends ParallelCommandGroup {
   /** Creates a new LacerAspirer. */
-  public LancerAspirer(Lanceur lanceur, Balayeuse balayeuse, LimeLight3 limeLight3, Led led) {
+  public LancerAspirer(Lanceur lanceur, Balayeuse balayeuse, Limelight3G limeLight3G) {
-    addCommands(new LancerBaseVitesse(lanceur, limeLight3), new Aspirer(balayeuse, led));
+    addCommands(new Lancer(lanceur, limeLight3G), new Aspirer(balayeuse));
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
   }

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

@@ -19,9 +19,9 @@ public class LancerBaseVitesse extends Command {
   private Timer timer;
   private double temp;
   /** Creates a new Lancer. */
-  public LancerBaseVitesse(Lanceur lanceur, LimeLight3 limeLight3) {
+  public LancerBaseVitesse(Lanceur lanceur, Limelight3G limeLight3G) {
     this.lanceur = lanceur;
-   // this.timer = new Timer();
+   this.timer = new Timer();
     this.limeLight3G = new Limelight3G();
     addRequirements(lanceur, limeLight3G);
     //this.temp = 0;
@@ -32,8 +32,8 @@ public class LancerBaseVitesse extends Command {
   @Override
   public void initialize() {
     pidController = new PIDController(0.0007, 0,0, 0.001);
-    //timer.reset();
+    timer.reset();
-    //timer.start();
+    timer.start();
     //temp = lanceur.Amp();    
   }
 
@@ -62,8 +62,8 @@ public class LancerBaseVitesse extends Command {
       double vitesse = lanceur.vitesseDemander();
       double output = pidController.calculate(lanceur.Vitesse(),vitesse);
       lanceur.Lancer(output);
-      if(lanceur.Vitesse() >= 1200){
+      if(timer.get() >1){
-        lanceur.Demeler(0.5);
+        lanceur.Demeler(1);
       } 
     // }
 
@@ -74,6 +74,8 @@ public class LancerBaseVitesse extends Command {
   public void end(boolean interrupted) {
     lanceur.Demeler(0);
     lanceur.Lancer(0);
+    timer.reset();
+    timer.stop();
   }
 
   // Returns true when the command should end.

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

@@ -27,35 +27,54 @@ public class Limelighter extends Command {
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
       .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
       .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+
   /** Creates a new Limelighter. */
-  public Limelighter(Limelight3G limelight3g,CommandSwerveDrivetrain drivetrain) {
+  public Limelighter(Limelight3G limelight3g, CommandSwerveDrivetrain drivetrain) {
     this.limelight3g = limelight3g;
     this.drivetrain = drivetrain;
-    addRequirements(drivetrain,limelight3g);
+    addRequirements(drivetrain, limelight3g);
+    timer = new Timer();
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    timer.reset();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     double[] BotPose = new double[6];
+    if (limelight3g.getV()) {
       BotPose = limelight3g.getBotPoseBlue();
-    botx = BotPose[0];
+      botx = BotPose[1];
-    boty = BotPose[1];
+      boty = BotPose[0];
-    angle = BotPose[5];
+      angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
-    drivetrain.setControl(drive.withRotationalRate(limelight3g.Calcule(4.625594, botx, 4.034536, boty, BotPose[5])/90));
+      if(angle >180){
-    if(limelight3g.Calcule(4.625594, botx, 4.034536, boty, angle)/90 < 0.1){
+        angle =- 360;
-      timer.start();
+      }
+      double calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 20;  
+      drivetrain.setControl(
+          drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
+      System.out.println(calcul);
+      if (calcul < 0.2 && calcul > -0.2) {
+    //     timer.start();
+    //   } else {
+    //     timer.reset();
+    //   }
+    // } else {
+    //   timer.stop();
+      drivetrain.setControl(drive.withRotationalRate(0));
+      }
     }
     else{
-      timer.reset();
+      drivetrain.setControl(drive.withRotationalRate(0));
     }
   }
 
+
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
@@ -66,6 +85,6 @@ public class Limelighter extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return timer.get() > 1;
+    return false;
   }
 }

src/main/java/frc/robot/commands/ModeAuto/AspirerAuto.java

@@ -24,13 +24,15 @@ public class AspirerAuto extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    balayeuse.Balayer(0.5);
+    balayeuse.BalayerEnbas(-0.5);
+    balayeuse.BalayerPadle(-0.2);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    balayeuse.Balayer(0);
+    balayeuse.BalayerEnbas(0);
+    balayeuse.BalayerPadle(0);
   }
 
   // Returns true when the command should end.

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

@@ -5,17 +5,14 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Balayeuse;
 import frc.robot.subsystems.Lanceur;
 /* 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 ModeOposer extends Command {
   private Lanceur lanceur;
-  private Balayeuse balayeuse;
   /** Creates a new Lancer. */
-  public ModeOposer(Lanceur lanceur, Balayeuse balayeuse) {
+  public ModeOposer(Lanceur lanceur) {
     this.lanceur = lanceur;
-    this.balayeuse = balayeuse;
+    addRequirements(lanceur);
-    addRequirements(lanceur ,balayeuse);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 

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

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Balayeuse;
+import frc.robot.subsystems.Lanceur;
+/* 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 ModeOposerBalayeuse extends Command {
+  private Balayeuse balayeuse;
+  /** Creates a new Lancer. */
+  public ModeOposerBalayeuse(Balayeuse balayeuse) {
+    this.balayeuse = balayeuse;
+    addRequirements(balayeuse);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {  
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    balayeuse.BalayerEnbas(0.5);
+    balayeuse.BalayerPadle(0.2);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    balayeuse.BalayerEnbas(0);
+    balayeuse.BalayerPadle(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -25,7 +25,7 @@ public class MonterBalyeuse extends Command {
   @Override
   public void execute() {
     if(!balayeuse.GetLimiSwtich()){
-      balayeuse.Pivoter(0.2);
+      balayeuse.Pivoter(0.5);
     }
     else{
       balayeuse.Reset();

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

@@ -25,7 +25,7 @@ public class MonterGrimpeur extends Command {
   public void execute() {
     
     if(Math.abs(grimpeur.Position()) < grimpeur.PositionFinal()){
-      grimpeur.Grimper(0.3);
+      grimpeur.Grimper(0.5);
       System.out.println("monte");
     }
     else {

src/main/java/frc/robot/subsystems/Balayeuse.java

@@ -22,11 +22,13 @@ public class Balayeuse extends SubsystemBase {
   SparkMax Pivot = new SparkMax(8, MotorType.kBrushless);
   DigitalInput limit = new DigitalInput(9);
   private GenericEntry EncodeurBalayeuse =
-  teb.add("Position bas balayeuse", 10).getEntry();
+  teb.add("Position bas balayeuse", 1.8).getEntry();
   private GenericEntry AmpBaleyeuse =
   teb.add("Ampérage Baleyeuse", 40).getEntry();
-  public void Balayer(double vitesse){
+  public void BalayerEnbas(double vitesse){
     Balaye1.set(vitesse);
+  }
+  public void BalayerPadle(double vitesse){
     Balaye2.set(vitesse);
   }
   public void Pivoter(double vitesse){
@@ -36,7 +38,7 @@ public class Balayeuse extends SubsystemBase {
     return Pivot.getEncoder().getPosition();
   } 
   public void Reset(){
-    Pivot.getEncoder().setPosition(0);
+    Pivot.getEncoder().setPosition(0.6);
   }
   public boolean GetLimiSwtich(){
     return !limit.get();
@@ -52,7 +54,7 @@ public class Balayeuse extends SubsystemBase {
     timer.start();
   }
   public double EncodeurBalayeuse(){
-    return EncodeurBalayeuse.getDouble(10);
+    return EncodeurBalayeuse.getDouble(1.8);
   }
   /** Creates a new Balayeuse. */
   public Balayeuse() {

src/main/java/frc/robot/subsystems/Lanceur.java

@@ -6,6 +6,7 @@ package frc.robot.subsystems;
 
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
 
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -16,7 +17,7 @@ public class Lanceur extends SubsystemBase {
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
   SparkFlex moteur1 = new SparkFlex(15, MotorType.kBrushless);
   SparkFlex moteur2 = new SparkFlex(18, MotorType.kBrushless);
-  SparkFlex Demeleur = new SparkFlex(19, MotorType.kBrushless);
+  SparkMax Demeleur = new SparkMax(19, MotorType.kBrushless);
   GenericEntry vitesse = 
   teb.add("vitesse lanceur",4000).getEntry();
   GenericEntry AmpLanceur = 

src/main/java/frc/robot/subsystems/LimeLight3.java

@@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.LimelightHelpers;
 
 public class LimeLight3 extends SubsystemBase {
-  NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+  NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
   NetworkTableEntry pipeline = table.getEntry("pipeline");
   /** Creates a new LimeLight3. */
   public LimeLight3() {
@@ -21,34 +21,34 @@ public class LimeLight3 extends SubsystemBase {
     }
   }
   public double[] getBotPoseBlue(){
-    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
+    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
     NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue");
     double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
     return BotPose;
     }
     public double[] getBotPoseRed(){
-      NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
+      NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balaie");
     NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired");
     double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
     return BotPose;
     }
     public double getTx(){
-      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
       NetworkTableEntry tx = table.getEntry("tx");
       return tx.getDouble(0.0);
     }
     public double getTId(){
-      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
       NetworkTableEntry tid = table.getEntry("tid");
       return tid.getDouble(0.0);
     }
     public double getTA(){
-      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balaie");
       NetworkTableEntry ta = table.getEntry("ta");
       return ta.getDouble(0.0);
     }
     public boolean getV(){
-      return LimelightHelpers.getTV("limelight-balon");
+      return LimelightHelpers.getTV("limelight-balaie");
     }
     public void AprilTag(){
       pipeline.setNumber(0);

src/main/java/frc/robot/subsystems/Limelight3G.java

@@ -22,13 +22,13 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
   }
   public double[] getBotPoseBlue(){
     NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
-    NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue");
+    NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
     double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
     return BotPose;
     }
     public double[] getBotPoseRed(){
       NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
-    NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired");
+    NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
     double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
     return BotPose;
     }
@@ -52,14 +52,7 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
     }
     public double Calcule(double x1, double x2, double y1, double y2, double angle)
     {
-      if (x1 > 4)
+      return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
-      {
-        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() {