limelight

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

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

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

@@ -13,10 +13,7 @@ import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.Limelight3G;
-
+import static edu.wpi.first.units.Units.*;
-import static edu.wpi.first.units.Units.MetersPerSecond;
-import static edu.wpi.first.units.Units.RadiansPerSecond;
-import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import java.util.Optional;
 
@@ -45,35 +42,44 @@ public class LimelighterAuto extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
+  public void initialize() {}
-    
-  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     double[] BotPose = new double[6];
+    System.out.println("e");
     if (limelight3g.getV()) {
-      if(!alliance.isPresent()){return;}
-        if(alliance.get() == Alliance.Blue){
         BotPose = limelight3g.getBotPoseBlue();
-      }
-      else{
-        BotPose = limelight3g.getBotPoseRed();
-      }
     
       botx = BotPose[1];
       boty = BotPose[0];
       angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
-      if(angle >180){
+      calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle);
-        angle -= 360;
+      if(calcul < -5 && calcul > -180){
+        drivetrain.setControl(
+          drive.withRotationalRate(0.5*180/Math.PI));
+      }
+      else if(calcul > 5 && calcul < 180){
+        drivetrain.setControl(
+          drive.withRotationalRate(-0.5*180/Math.PI));
+      }
+      else if(calcul >= 180){
+        drivetrain.setControl(
+          drive.withRotationalRate(-0.5*180/Math.PI));
+      }
+      else if(calcul <= -180){
+        drivetrain.setControl(
+          drive.withRotationalRate(0.5*180/Math.PI));
+      }
+      else{
+        drivetrain.setControl(
+          drive.withRotationalRate(0));
       }  
-      calcul = limelight3g.Calcule(botx, 4, boty, 4.6, angle) / 5;  
       drivetrain.setControl(
           drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul));
-              System.out.println("Limelighter");
+      System.out.println(angle);
-      System.out.println(calcul);
+      if (calcul < 0.2 && calcul > -0.2) {
-      if (calcul < 0.3 && calcul > -0.3) {
       drivetrain.setControl(drive.withRotationalRate(0));
       }
     }
@@ -92,7 +98,7 @@ public class LimelighterAuto extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return calcul < 0.3 && calcul > -0.3;
+    return calcul < 5 && calcul > -5;
   }
 }
 

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

@@ -52,7 +52,22 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag")
     }
     public double Calcule(double x1, double x2, double y1, double y2, double angle)
     {
-      return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
+      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(90-((x2 - x1) / (y2 - y1)))* (180 / Math.PI)+270 - angle;
+        }
+        else{
+          return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+180 - angle;
+        }
+      }
     }
   @Override
   public void periodic() {