apriltag

src/main/deploy/pathplanner/autos/BlueHaut1.auto

@@ -19,34 +19,9 @@
                 "data": {
                   "name": "AprilTag"
                 }
-              },
-              {
-                "type": "sequential",
-                "data": {
-                  "commands": [
-                    {
-                      "type": "wait",
-                      "data": {
-                        "waitTime": 0.5
-                      }
-                    },
-                    {
-                      "type": "named",
-                      "data": {
-                        "name": "L4"
-                      }
-                    }
-                  ]
-                }
               }
             ]
           }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "CoralExpire"
-          }
         }
       ]
     }

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

@@ -11,6 +11,7 @@ import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.MathUtil;
@@ -84,7 +85,6 @@ public class RobotContainer {
   public double getAngle() {
     return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot
   }
-
   Elevateur elevateur = new Elevateur();
   Pince pince = new Pince();
   ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY);
@@ -98,6 +98,7 @@ public class RobotContainer {
   autoChooser = AutoBuilder.buildAutoChooser();
   CameraServer.startAutomaticCapture();
    SmartDashboard.putData("Auto Chooser", autoChooser);
+   //NamedCommands.registerCommand("AprilTag", new AprilTag3(limelight3, drivetrain,, null));
   configureBindings();
   }
 
@@ -116,7 +117,7 @@ public class RobotContainer {
     manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
     manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie));
     manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie));
-    // manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain1,manette1::getLeftX,manette1::getLeftY));
+    manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
     manette1.povRight().whileTrue(new CoralExpire(pince, bougie));
     manette1.leftTrigger().whileTrue(new DepartPince(pince));
     manette1.povDown().whileTrue(new Algue_inspire(pince,bougie));
@@ -140,8 +141,8 @@ public class RobotContainer {
     manette2.x().whileTrue(new ExpireCorail(requin, bougie));
 
     //limelight
-    // manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
+    manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
-    // manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
+    manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
     
     //Pince manuel
     pince.setDefaultCommand(new RunCommand(()->{

src/main/java/frc/robot/commands/Limelight/AprilTag3.java

@@ -23,7 +23,7 @@ import frc.robot.subsystems.Limelight3;
 /* 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 AprilTag3 extends Command {
   private Limelight3 limelight3;
-  private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  private CommandSwerveDrivetrain drivetrain;
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
   private DoubleSupplier x;
@@ -61,31 +61,42 @@ public class AprilTag3 extends Command {
       BotPose = limelight3.getBotPoseBlue();
     }
     double botx = BotPose[0];
+    System.out.println(botx);
     double boty = BotPose[1];
     double tx = limelight3.getTx();
     double tagId = limelight3.getTId();
-    if(limelight3.getV() == true){
-      if(tagId ==10){
         drivetrain.setControl(drive.
         withRotationalRate(tx/20).
         withVelocityX((botx-5.81)*2).
         withVelocityY((boty-4)*4));
-      }
+    // if(limelight3.getV() == true){
-      else if(tagId ==7){
+    //   if(tagId ==10){
-        drivetrain.setControl(drive.
+    //     drivetrain.setControl(drive.
-        withRotationalRate(tx/20).
+    //     withRotationalRate(tx/20).
-        withVelocityX(2-botx).
+    //     withVelocityX((botx-5.81)*2).
-        withVelocityY(2-boty));
+    //     withVelocityY((boty-4)*4));
+    //   }
+    //   else if(tagId ==7){
+    //     drivetrain.setControl(drive.
+    //     withRotationalRate(tx/20).
+    //     withVelocityX(2-botx).
+    //     withVelocityY(2-boty));
 
-      }
+    //   }
-  }
+  // }
-  else{
+  // if(limelight3.getV()){
-      drivetrain.setControl(drive.
+  //   drivetrain.setControl(drive.
-      withRotationalRate(0).
+  //   withRotationalRate(limelight3.getTx()/10).
-      withVelocityX(0).
+  //   withVelocityX(x.getAsDouble()).
-      withVelocityY(0));
+  //   withVelocityY(y.getAsDouble()));
-    }
+  // }
-  }
+  // else{
+  //     drivetrain.setControl(drive.
+  //     withRotationalRate(0).
+  //     withVelocityX(0).
+  //     withVelocityY(0));
+  //   }
+   }
 
   // Called once the command ends or is interrupted.
   @Override

src/main/java/frc/robot/commands/Limelight/AprilTag3G.java

@@ -18,7 +18,7 @@ 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 AprilTag3G extends Command {
   private Limelight3G limelight3g;
-  private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  private CommandSwerveDrivetrain drivetrain;
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
   private DoubleSupplier x;

src/main/java/frc/robot/commands/Limelight/Forme3.java

@@ -20,7 +20,7 @@ public class Forme3 extends Command {
   private Limelight3 limelight3;
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
-  private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+  private CommandSwerveDrivetrain drivetrain;
   private DoubleSupplier x;
   private DoubleSupplier y;
     /* Setting up bindings for necessary control of the swerve drive platform */

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

@@ -30,13 +30,13 @@ public class Limelight3 extends SubsystemBase {
   }
   public double[] getBotPoseBlue(){
   NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
-  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
+  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");
-  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
+  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired");
   double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
   return BotPose;
   }