Merge branch 'main' of https://demerso.net/pls5618/2023/robot

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

@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -16,20 +17,25 @@ import frc.robot.subsystems.Gratte;
 import frc.robot.subsystems.bras.BrasTelescopique;
 import frc.robot.subsystems.bras.Pince;
 import frc.robot.subsystems.bras.Pivot;
+import pabeles.concurrency.ConcurrencyOps.Reset;
 import frc.robot.subsystems.Limelight;
 // command
 import frc.robot.commands.BrakeFerme;
 import frc.robot.commands.BrakeOuvre;
+import frc.robot.commands.Cone;
+import frc.robot.commands.Cube;
 import frc.robot.commands.GratteBaisser;
 import frc.robot.commands.GratteMonte;
 import frc.robot.commands.Gyro;
 import frc.robot.commands.Reculer;
+import frc.robot.commands.Tape;
 import frc.robot.commands.bras.FermePince;
 import frc.robot.commands.bras.OuvrePince;
 import frc.robot.commands.bras.PivotBrasRentre;
 import frc.robot.commands.bras.PivoteBrasBas;
 import frc.robot.commands.bras.PivoteBrasHaut;
 import frc.robot.commands.bras.PivoteBrasMilieux;
+import frc.robot.commands.Apriltag;
 
 public class RobotContainer {
 CommandXboxController manette1 = new CommandXboxController(0);
@@ -53,7 +59,10 @@ PivotBrasRentre pivotBrasRentre = new PivotBrasRentre(brasTelescopique, pivot);
 PivoteBrasBas pivoteBrasBas = new PivoteBrasBas(brasTelescopique, pivot);
 PivoteBrasMilieux pivoteBrasMilieux = new PivoteBrasMilieux(brasTelescopique, pivot);
 PivoteBrasHaut pivoteBrasHaut = new PivoteBrasHaut(brasTelescopique, pivot);
-
+Cone cone = new Cone(limelight, basePilotable, ()->-manette1.getLeftY());
+Cube cube = new Cube(limelight, basePilotable, ()->-manette1.getLeftY());
+Apriltag aprilTag = new Apriltag(limelight, basePilotable, ()->-manette1.getLeftY());
+Tape tape = new Tape(limelight, basePilotable, ()->-manette1.getLeftY());
 public RobotContainer() {
     configureBindings();
 
@@ -66,7 +75,9 @@ public RobotContainer() {
   private void configureBindings() {
     manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince));
     manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable));
-
+    manette1.y().whileTrue(gyro);
+    manette1.b().toggleOnTrue(Commands.startEnd());
+    manette1.start().toggleOnTrue(Commands.startEnd(basePilotable::resetGyro, basePilotable::resetGyro, basePilotable));
 
   }
 

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

@@ -0,0 +1,49 @@
+// 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 java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.BasePilotable;
+import frc.robot.subsystems.Limelight;
+
+public class Apriltag extends CommandBase {
+  private Limelight limelight;
+  private BasePilotable basePilotable;
+  private DoubleSupplier doubleSupplier;
+  /** Creates a new Apriltag. */
+  public Apriltag(Limelight limelight,BasePilotable basePilotable,DoubleSupplier doubleSupplier) {
+    this.basePilotable = basePilotable;
+    this.limelight = limelight;
+    this.doubleSupplier = doubleSupplier;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(limelight,basePilotable);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    limelight.apriltag();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    limelight.pilote();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,49 @@
+// 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 java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.BasePilotable;
+import frc.robot.subsystems.Limelight;
+
+public class Cone extends CommandBase {
+  private Limelight limelight;
+  private BasePilotable basePilotable;
+  private DoubleSupplier doubleSupplier;
+  /** Creates a new ConeCube. */
+  public Cone(Limelight limelight,BasePilotable basePilotable,DoubleSupplier doubleSupplier) {
+    this.basePilotable = basePilotable;
+    this.limelight = limelight;
+    this.doubleSupplier = doubleSupplier;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(limelight,basePilotable);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    limelight.cone();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    limelight.pilote();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -0,0 +1,49 @@
+// 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 java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.BasePilotable;
+import frc.robot.subsystems.Limelight;
+
+public class Cube extends CommandBase {
+  private Limelight limelight;
+  private BasePilotable basePilotable;
+  private DoubleSupplier doubleSupplier;
+  /** Creates a new cube. */
+  public Cube(Limelight limelight,BasePilotable basePilotable,DoubleSupplier doubleSupplier) {
+    this.basePilotable = basePilotable;
+    this.limelight = limelight;
+    this.doubleSupplier = doubleSupplier;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(limelight,basePilotable);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    limelight.cube();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    limelight.pilote();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -4,6 +4,8 @@
 
 package frc.robot.commands;
 
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import frc.robot.Constants;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Gratte;
 
@@ -24,7 +26,7 @@ public class GratteMonte extends CommandBase {
   @Override
   public void execute() {
     if(gratte.hautd()){
-      gratte.Lever(0);
+      gratte.Lever(0.5);
     }
     else{
       gratte.Lever(0.5);
@@ -43,7 +45,7 @@ public class GratteMonte extends CommandBase {
 
   // Returns true when the command should end.
   @Override
-  public boolean isFinished() {
+  public boolean isFinished(){
     return false;
-  }
 }
+  }

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

@@ -0,0 +1,49 @@
+// 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 java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.BasePilotable;
+import frc.robot.subsystems.Limelight;
+
+public class Tape extends CommandBase {
+  private Limelight limelight;
+  private BasePilotable basePilotable;
+  private DoubleSupplier doubleSupplier;
+  /** Creates a new Tape. */
+  public Tape(Limelight limelight,BasePilotable basePilotable,DoubleSupplier doubleSupplier) {
+    this.basePilotable = basePilotable;
+    this.limelight = limelight;
+    this.doubleSupplier = doubleSupplier;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(limelight,basePilotable);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    limelight.tape();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    limelight.pilote();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -8,6 +8,7 @@ import com.kauailabs.navx.frc.AHRS;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -59,6 +60,10 @@ public void BrakeFerme(){
   brakedroit.set(Value.kReverse);
   brakegauche.set(Value.kReverse);
 }
+public void resetGyro(){
+  try {gyroscope.reset();} catch(Exception e){DriverStation.reportError("bye bye",true);
+ }
+  }
   /** Creates a new BasePilotable. */
   public BasePilotable() {
     droit.setInverted(true);

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

@@ -17,6 +17,7 @@ public class Gratte extends SubsystemBase {
   private DigitalInput limithg = new DigitalInput(Constants.limithg);
   private DigitalInput limitbd = new DigitalInput(Constants.limitbd);
   private DigitalInput limitbg = new DigitalInput(Constants.limitbg);
+  
   public boolean hautd(){
     return limithd.get();
   }

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

@@ -8,6 +8,7 @@ import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import org.photonvision.PhotonCamera;
 import org.photonvision.common.hardware.VisionLEDMode;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
 import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -40,6 +41,18 @@ public class Limelight extends SubsystemBase {
     limelight.setPipelineIndex(1);
   }
 
+  public double getYaw() {
+    var result = limelight.getLatestResult();
+    if(result.hasTargets()){
+    return  result.getBestTarget().getYaw();
+    }
+    return 0;
+  
+  }
+  public void pilote(){
+    limelight.setLED(VisionLEDMode.kOff);
+    limelight.setDriverMode(true);
+  }
   @Override
   public void periodic() {
     CameraServer.startAutomaticCapture();

vendordeps/photonlib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "photonlib.json",
     "name": "photonlib",
-    "version": "v2023.3.0",
+    "version": "v2023.4.2",
     "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
     "mavenUrls": [
         "https://maven.photonvision.org/repository/internal",
@@ -13,7 +13,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "PhotonLib-cpp",
-            "version": "v2023.3.0",
+            "version": "v2023.4.2",
             "libName": "Photon",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -30,12 +30,12 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "PhotonLib-java",
-            "version": "v2023.3.0"
+            "version": "v2023.4.2"
         },
         {
             "groupId": "org.photonvision",
             "artifactId": "PhotonTargeting-java",
-            "version": "v2023.3.0"
+            "version": "v2023.4.2"
         }
     ]
 }