Code composante fini

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

@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -16,18 +17,21 @@ import frc.robot.commands.MonterGrimpeur;
 import frc.robot.subsystems.Balayeuse;
 import frc.robot.subsystems.Grimpeur;
 import frc.robot.subsystems.Lanceur;
+import frc.robot.subsystems.LimeLight3;
 
 public class RobotContainer {
   Balayeuse balayeuse = new Balayeuse();
   Grimpeur grimpeur = new Grimpeur();
   Lanceur lanceur = new Lanceur();
+  LimeLight3 limeLight3 = new LimeLight3();
   CommandXboxController manette = new CommandXboxController(0);
   public RobotContainer() {
+    CameraServer.startAutomaticCapture();
     configureBindings();
   }
 
   private void configureBindings() {
-    manette.a().whileTrue(new Lancer(lanceur));
+    manette.a().whileTrue(new Lancer(lanceur,limeLight3));
     manette.rightTrigger().whileTrue(new MonterGrimpeur(grimpeur));
     manette.leftTrigger().whileTrue(new DescendreGrimpeur(grimpeur));
     manette.rightBumper().whileTrue(new MonterBalyeuse(balayeuse));

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

@@ -9,7 +9,7 @@ import frc.robot.subsystems.Balayeuse;
 
 /* 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 {
-  Balayeuse balayeuse;
+  private Balayeuse balayeuse;
   /** Creates a new Aspirer. */
   public Aspirer(Balayeuse balayeuse) {
     this.balayeuse = balayeuse;

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

@@ -9,7 +9,7 @@ import frc.robot.subsystems.Balayeuse;
 
 /* 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 DescendreBalyeuse extends Command {
-  Balayeuse balayeuse;
+  private Balayeuse balayeuse;
   /** Creates a new Descendre. */
   public DescendreBalyeuse(Balayeuse balayeuse) {
     this.balayeuse = balayeuse;

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

@@ -9,7 +9,7 @@ import frc.robot.subsystems.Grimpeur;
 
 /* 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 DescendreGrimpeur extends Command {
-  Grimpeur grimpeur;  
+  private Grimpeur grimpeur;  
   /** Creates a new DescendreGrimpeur. */
   public DescendreGrimpeur(Grimpeur grimpeur) {
     this.grimpeur = grimpeur;
@@ -28,6 +28,7 @@ public class DescendreGrimpeur extends Command {
     }
     else{
       grimpeur.Reset();
+      grimpeur.Grimper(0);
     }
   }
 

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

@@ -4,28 +4,37 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Lanceur;
+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 Lancer extends Command {
-  Lanceur lanceur;
+  private Lanceur lanceur;
+  private PIDController pidController;
+  private LimeLight3 limeLight3;
+  private double output;
   /** Creates a new Lancer. */
-  public Lancer(Lanceur lanceur) {
+  public Lancer(Lanceur lanceur, LimeLight3 limeLight3) {
     this.lanceur = lanceur;
+    this.limeLight3 = new LimeLight3();
     addRequirements(lanceur);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    pidController = new PIDController(0, 0,0, 0);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = 0.8;
+    double vitesse = (100-limeLight3.getTA())/100;
-    lanceur.Lancer(vitesse);
+    double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+    lanceur.Lancer(output);
     if(lanceur.Vitesse() >= vitesse){
       lanceur.Demeler(0.5);
     }

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

@@ -9,7 +9,7 @@ import frc.robot.subsystems.Balayeuse;
 
 /* 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 MonterBalyeuse extends Command {
-  Balayeuse balayeuse;
+  private Balayeuse balayeuse;
   /** Creates a new MonterBalyeuse. */
   public MonterBalyeuse(Balayeuse balayeuse) {
     this.balayeuse = balayeuse;
@@ -29,6 +29,7 @@ public class MonterBalyeuse extends Command {
     }
     else{
       balayeuse.Reset();
+      balayeuse.Pivoter(0);
     }
   }
 

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

@@ -9,7 +9,7 @@ import frc.robot.subsystems.Grimpeur;
 
 /* 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 MonterGrimpeur extends Command {
-  Grimpeur grimpeur;
+  private Grimpeur grimpeur;
   /** Creates a new MonterGrimpeur. */
   public MonterGrimpeur(Grimpeur grimpeur) {
     this.grimpeur = grimpeur;
@@ -23,7 +23,7 @@ public class MonterGrimpeur extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(grimpeur.Position() > 10){
+    if(grimpeur.Position() < 10){
       grimpeur.Grimper(0.5);
     }
   }

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

@@ -6,9 +6,7 @@ package frc.robot.subsystems;
 
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -19,8 +17,8 @@ public class Grimpeur extends SubsystemBase {
   DigitalInput limit = new DigitalInput(1);
   public void Grimper(double vitesse){
     grimpeur1.set(vitesse);
-    slaveConfig.follow(grimpeur1, false);
+    grimpeur2.set(vitesse);
-    grimpeur2.configure(slaveConfig,null,null);
+
   }
   public double Position(){
     return grimpeur1.getEncoder().getPosition();

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

@@ -6,7 +6,6 @@ package frc.robot.subsystems;
 
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkMaxConfig;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -14,11 +13,9 @@ public class Lanceur extends SubsystemBase {
     SparkFlex moteur1 = new SparkFlex(2, MotorType.kBrushless);
     SparkFlex moteur2 = new SparkFlex(3, MotorType.kBrushless);
     SparkFlex Demeleur = new SparkFlex(4, MotorType.kBrushless);
-    SparkMaxConfig slaveConfig = new SparkMaxConfig();
     public void Lancer(double vitesse){
         moteur1.set(vitesse);
-        slaveConfig.follow(moteur1, true);
+        moteur2.set(-vitesse);
-        moteur2.configure(slaveConfig,null,null);
     }
     public void Demeler(double vitesse){
       Demeleur.set(vitesse);

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

@@ -0,0 +1,74 @@
+// 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.subsystems;
+
+import edu.wpi.first.net.PortForwarder;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.LimelightHelpers;
+
+public class LimeLight3 extends SubsystemBase {
+    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+  NetworkTableEntry pipeline = table.getEntry("pipeline");
+  /** Creates a new LimeLight3. */
+  public LimeLight3() {
+        for(int port = 5800; port <=5807; port++){
+      PortForwarder.add(port, "limelight.local", port);
+    }
+  }
+  public double[] getBotPoseBlue(){
+    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
+    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_orb_wpired");
+    double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
+    return BotPose;
+    }
+    public double getTx(){
+      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+      NetworkTableEntry tx = table.getEntry("tx");
+      return tx.getDouble(0.0);
+    }
+    public double getTId(){
+      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+      NetworkTableEntry tid = table.getEntry("tid");
+      return tid.getDouble(0.0);
+    }
+    public double getTA(){
+      NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+      NetworkTableEntry ta = table.getEntry("ta");
+      return ta.getDouble(0.0);
+    }
+    public boolean getV(){
+      return LimelightHelpers.getTV("limelight-balon");
+    }
+    public void AprilTag(){
+      pipeline.setNumber(0);
+    }
+    public void Forme(){
+      pipeline.setNumber(1);
+    }
+    public double Calcule(double x1, double x2, double y1, double y2, double angle)
+    {
+      if (x1 > 4)
+      {
+        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() {
+    // This method will be called once per scheduler run
+  }
+}