pince + main

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

@@ -21,18 +21,26 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.TunerConstants.TunerConstants;
 import frc.robot.commands.AprilTag3;
 import frc.robot.commands.AprilTag3G;
+import frc.robot.commands.ElevateurManuel;
 import frc.robot.commands.Forme3;
+import frc.robot.commands.L2;
+import frc.robot.commands.PinceManuel;
+import frc.robot.commands.PinceManuel2;
 import frc.robot.commands.RainBow;
+import frc.robot.commands.reset;
 import frc.robot.subsystems.Bougie;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Elevateur;
 import frc.robot.subsystems.Limelight3;
 import frc.robot.subsystems.Limelight3G;
+import frc.robot.subsystems.Pince;
 
 public class RobotContainer {
     private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
@@ -49,7 +57,11 @@ public class RobotContainer {
     private final CommandXboxController manette2 = new CommandXboxController(1);
    
     public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
-    
+    Elevateur elevateur = new Elevateur();
+    Pince pince = new Pince();
+    ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY);
+    PinceManuel pinceManuel = new PinceManuel(pince);
+    PinceManuel2 pinceManuel2 = new PinceManuel2(pince);
     private final SendableChooser<Command> autoChooser;
     Bougie bougie = new Bougie();
     Limelight3G limelight3g = new Limelight3G();
@@ -65,8 +77,13 @@ public class RobotContainer {
         configureBindings();
         NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null));
     }
-
     private void configureBindings() {
+      elevateur.setDefaultCommand(new RunCommand(()->{
+        elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2));
+      }, elevateur));
+      pince.setDefaultCommand(new RunCommand(()->{
+        pince.pivote(MathUtil.applyDeadband(manette2.getRightY()/5, 0.2));
+      }, pince));
         // Note that X is defined as forward according to WPILib convention,
         // and Y is defined as to the left according to WPILib convention.
         drivetrain.setDefaultCommand(
@@ -78,15 +95,19 @@ public class RobotContainer {
             )
         );
 
+        
         // reset the field-centric heading on left bumper press
-        manette1.y().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-        manette1.a().whileTrue(drivetrain.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-        manette1.b().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kForward));
-        manette1.x().whileTrue(drivetrain.sysIdDynamic(SysIdRoutine.Direction.kReverse));
         manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
         manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
         manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
+        manette1.a().whileTrue(new reset(elevateur));
+        manette1.b().whileTrue(new L2(elevateur, pince));
+        
+
         manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
+
+
+
         drivetrain.registerTelemetry(logger::telemeterize);
     }
 

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

@@ -28,25 +28,25 @@ public class L2 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(elevateur.position()>=500 && elevateur.position()<=510){
+    if(elevateur.position()<=-1 && elevateur.position()>=-0.9){
       elevateur.vitesse(0);
     }
-    else if(elevateur.position()>=510){
-      elevateur.vitesse(0.3);
+    else if(elevateur.position()>=-1){
+      elevateur.vitesse(-0.2);
     }
     else{
-      elevateur.vitesse(.3);
-    }
-    if(pince.encodeurpivot()>=500 && pince.encodeurpivot()<=510){
-      pince.pivote(0);
+      elevateur.vitesse(.2);
+    // }
+    // if(pince.encodeurpivot()>=500 && pince.encodeurpivot()<=510){
+    //   pince.pivote(0);
      
-    }
-    else if(pince.encodeurpivot()>=510){
-      pince.pivote(0.2);
-    }
-    else{
-      pince.pivote(-0.2);
-    }
+    // }
+    // else if(pince.encodeurpivot()>=510){
+    //   pince.pivote(0.2);
+    // }
+    // else{
+    //   pince.pivote(-0.2);
+     }
 
   }
 

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

@@ -7,17 +7,19 @@ package frc.robot.subsystems;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 public class Elevateur extends SubsystemBase {
+  ShuffleboardTab teb = Shuffleboard.getTab("teb");
   /** Creates a new Elevateur. */
-  public Elevateur() {}
+  public Elevateur() {
+    teb.addDouble("encodeur elevateur",this::position);
+  }
   final SparkMax  monte = new SparkMax(22, MotorType.kBrushless);
   final DigitalInput limit2 = new DigitalInput(1);
-  GenericEntry teb = Shuffleboard.getTab("teb")
-  .add("elevateur encodeur",position())
-  .getEntry();
+  
   public double position(){
     return monte.getEncoder().getPosition();
   }