debug

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

@@ -41,9 +41,17 @@ import frc.robot.commands.Pince.CorailAspir;
 import frc.robot.commands.Pince.CoralAlgueInspire;
 import frc.robot.commands.Pince.CoralExpire;
 import frc.robot.commands.Pince.PinceManuel;
+import frc.robot.commands.grimpeur.GrimperHaut;
+import frc.robot.commands.grimpeur.GrimpeurBas;
 import frc.robot.commands.grimpeur.GrimpeurManuelhaut;
+import frc.robot.commands.grimpeur.ResetGrimpeur;
+import frc.robot.commands.requin.BalayeuseAlgue;
 import frc.robot.commands.requin.BalayeuseBas;
+import frc.robot.commands.requin.BalayeuseCoral;
 import frc.robot.commands.requin.BalayeuseHaut;
+import frc.robot.commands.requin.ExpireAlgue;
+import frc.robot.commands.requin.L1Requin;
+import frc.robot.commands.requin.aspire;
 import frc.robot.commands.requin.exspire;
 import frc.robot.subsystems.Bougie;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -84,7 +92,7 @@ public class RobotContainer {
   Pose2d pose = new Pose2d();
   Grimpeur Grimpeur = new Grimpeur();
   Requin requin = new Requin();
-  
+  CorailAspir corailAspir = new CorailAspir(pince, bougie);
   public RobotContainer() {
     autoChooser = AutoBuilder.buildAutoChooser("New Auto");
     SmartDashboard.putData("Auto Mode", autoChooser);
@@ -117,7 +125,7 @@ public class RobotContainer {
     manette1.rightBumper().whileTrue(new StationPince(pince, elevateur));
     manette1.leftTrigger().whileTrue(new CoralExpire(pince, bougie));
     manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
-
+    manette1.povRight().whileTrue(new AlgueExpire(pince, bougie));
      //elevateur
      manette1.a().whileTrue(new Depart(elevateur, pince));
      manette1.b().whileTrue(new L2(elevateur,pince));
@@ -128,11 +136,12 @@ public class RobotContainer {
     //requin
     manette2.a().whileTrue(new CorailAspir(pince,bougie));
     manette2.b().whileTrue(new Algue_inspire(pince,bougie));
-    manette2.y().whileTrue(new BalayeuseHaut(requin));
+    manette2.y().whileTrue(new BalayeuseAlgue(requin,bougie));
-    manette2.x().whileTrue(new BalayeuseBas(requin));
+    manette2.x().whileTrue(new BalayeuseHaut(requin));
-    manette2.rightTrigger().whileTrue(new exspire(requin,bougie));
+    manette2.rightTrigger().whileTrue(new ExpireAlgue(requin, bougie));
-    manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie));
+    manette2.leftTrigger().whileTrue(new BalayeuseCoral(requin, bougie));
-
+    manette1.povUp().whileTrue(new L1Requin(requin, bougie));
+    manette2.povUp().whileTrue(new aspire(requin, bougie));
      //Pince manuel
      pince.setDefaultCommand(new RunCommand(()->{
       pince.pivote(MathUtil.applyDeadband(manette2.getRightY()*manette2.getRightY()*manette2.getRightY(), 0.05));
@@ -149,8 +158,10 @@ public class RobotContainer {
 
     //Reset encodeur
     manette2.start().whileTrue(new reset(elevateur, pince));
-    //grimpeur manuel
+    //grimpeur
     manette2.povDown().whileTrue(new GrimpeurManuelhaut(Grimpeur, bougie));
+    manette2.povLeft().whileTrue(new GrimperHaut(Grimpeur, bougie));
+    manette2.povRight().whileTrue(new GrimpeurBas(Grimpeur));
   }
     
     public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/Elevateur/Depart.java

@@ -30,7 +30,7 @@ public class Depart extends Command {
       elevateur.reset();
     }
     else{
-      elevateur.vitesse(.5);
+      elevateur.vitesse(.3);
     }
     
   }

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

@@ -26,24 +26,15 @@ public class L2 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(elevateur.position()<=elevateur.encodeurelevateurL2bas() && elevateur.position()>=elevateur.encodeurelevateurL2haut()){
+     if(pince.encodeurpivot()>=15.6 && pince.encodeurpivot()<=16){
-      elevateur.vitesse(0);
+       pince.pivote(0);
-    }
-    else if(elevateur.position()>=elevateur.encodeurelevateurL2bas()){
-      elevateur.vitesse(-0.2);
-    }
-    else{
-      elevateur.vitesse(.2);
-    // }
-    // if(pince.encodeurpivot()>=500 && pince.encodeurpivot()<=510){
-    //   pince.pivote(0);
      
-    // }
+     }
-    // else if(pince.encodeurpivot()>=510){
+     else if(pince.encodeurpivot()>=16){
-    //   pince.pivote(0.2);
+       pince.pivote(-0.1);
-    // }
+     }
-    // else{
+     else{
-    //   pince.pivote(-0.2);
+       pince.pivote(0.1);
      }
 
   }

src/main/java/frc/robot/commands/Elevateur/L3.java

@@ -32,20 +32,20 @@ public class L3 extends Command {
       elevateur.vitesse(0);
     }
     else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){
-      elevateur.vitesse(-0.2);
+      elevateur.vitesse(-0.5);
     }
     else{
-      elevateur.vitesse(.2);
+      elevateur.vitesse(0.5);
     }
-    // if(pince.encodeurpivot()>=700 && pince.encodeurpivot()<=710){
+     if(pince.encodeurpivot()>=15.6 && pince.encodeurpivot()<=16){
-    //   pince.pivote(0);
+       pince.pivote(0);
-    // }
+     }
-    // else if(pince.encodeurpivot()>=710){
+     else if(pince.encodeurpivot()>=16){
-    //   pince.pivote(0.2);
+       pince.pivote(-0.1);
-    // }
+     }
-    // else{
+     else{
-    //   pince.pivote(-0.2);
+       pince.pivote(0.1);
-    // }
+     }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/Elevateur/L4.java

@@ -29,22 +29,23 @@ public class L4 extends Command {
   public void execute() {
     if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){
       elevateur.vitesse(0);
+      
     }
     else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){
-      elevateur.vitesse(-0.2);
+      elevateur.vitesse(-0.5);
     }
     else{
-      elevateur.vitesse(.2);
+      elevateur.vitesse(.5);
     }
-    // if(pince.encodeurpivot()>=800 && pince.encodeurpivot()<=809){
+     if(pince.encodeurpivot()>=21.76 && pince.encodeurpivot()<=22.3){
-    //   pince.pivote(0);
+       pince.pivote(0);
-    // }
+     }
-    // else if(pince.encodeurpivot()>=810){
+     else if(pince.encodeurpivot()>=22.3){
-    //   pince.pivote(0.2);
+       pince.pivote(-0.1);
-    // }
+     }
-    // else{
+     else{
-    //   pince.pivote(-0.2);
+       pince.pivote(0.1);
-    // }
+     }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/Elevateur/StationPince.java

@@ -31,23 +31,14 @@ public class StationPince extends Command {
   @Override
   public void execute() {
     pince.aspirecoral(0.5);
-    if(pince.encodeurpivot()<=elevateur.encodeurelevateurstationbas() && pince.encodeurpivot()>=elevateur.encodeurelevateurstationhaut()){
+    if(pince.encodeurpivot()<=9.8 && pince.encodeurpivot()>=12){
       pince.pivote(0);
     }
-    else if(pince.encodeurpivot()>=elevateur.encodeurelevateurstationbas()){
+    else if(pince.encodeurpivot()>=9.8){
-      pince.pivote(0.2);
+      pince.pivote(-0.1);
     }
     else{
-      pince.pivote(-0.2);
+      pince.pivote(0.1);
-    }
-    if(elevateur.position()>=400 && elevateur.position()<=410){
-      elevateur.vitesse(0);
-    }
-    else if(elevateur.position()>=410){
-      elevateur.vitesse(0.3);
-    }
-    else{
-      elevateur.vitesse(-.3);
     }
   }
 

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

@@ -48,10 +48,10 @@ public class AprilTag3G extends Command {
     double a = limelight3g.getX();
     if(limelight3g.getV() == true){
       drivetrain.setControl(drive.
-      withRotationalRate(-a/5).
+      withRotationalRate(-a/7).
       withVelocityX(x.getAsDouble()).
       withVelocityY(y.getAsDouble()));  
-       System.out.println(a/5);
+       System.out.println(a/7);
     }
     else{
       drivetrain.setControl(drive.

src/main/java/frc/robot/commands/Pince/CorailAspir.java

@@ -27,11 +27,14 @@ public class CorailAspir extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(pince.emperagecoral() > 60){
+    if(pince.emperagecoral() > 15){
       pince.aspirecoral(0);
       bougie.Bleu();
     }
-    pince.aspirecoral(0.5);
+    else{
+     pince.aspirecoral(0.5); 
+    }
+    
   }
 
   // Called once the command ends or is interrupted.
@@ -43,6 +46,6 @@ public class CorailAspir extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return pince.emperagecoral()>13;
   }
 }

src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java

@@ -23,10 +23,10 @@ public class GrimpeurBas extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(grimpeur.encodeur()>=-38.5 && grimpeur.encodeur()<=-39.19){
+    if(grimpeur.encodeur()>=-43.3 && grimpeur.encodeur()<=-42.5){
       grimpeur.grimpe(0);
     }
-    else if(grimpeur.encodeur()>=-38.5){
+    else if(grimpeur.encodeur()>=-43.3){
       grimpeur.grimpe(-0.5);
     }
    else{grimpeur.grimpe(0.5);

src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java

@@ -27,7 +27,7 @@ public class BalayeuseAlgue extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-     if(requin.encodeur()>=500 &&  requin.encodeur()<=510)
+     if(requin.encodeur()<=715 &&  requin.encodeur()>=700)
      {
        requin.rotationer(0);
        if(requin.amp()> 60){
@@ -36,10 +36,10 @@ public class BalayeuseAlgue extends Command {
      }
       else
       {
-           requin.balaye(0.5);
+           requin.balaye(-0.5);
       }
      }
-     else if(requin.encodeur()>=510){
+     else if(requin.encodeur()>=700){
      requin.rotationer(0.5);
      }
      else{

src/main/java/frc/robot/commands/requin/BalayeuseBas.java

@@ -24,7 +24,7 @@ public class BalayeuseBas extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-  requin.rotationer(-0.5);
+  requin.rotationer(0.5);
   }
   // Called once the command ends or is interrupted.
   @Override

src/main/java/frc/robot/commands/requin/BalayeuseCoral.java

@@ -27,7 +27,7 @@ public class BalayeuseCoral extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-     if(requin.encodeur()>=100 &&  requin.encodeur()<=110){
+     if(requin.encodeur()<=1200 &&  requin.encodeur()>=1050){
        requin.rotationer(0);
       if(requin.amp()>60){
       requin.balaye(0);
@@ -36,18 +36,18 @@ public class BalayeuseCoral extends Command {
         requin.rotationer(0);
        }
         else{
-          requin.rotationer(0.5);
+          requin.rotationer(-0.5);
         }
     }
      else{
        requin.balaye(0.5);
      }
      }
-     else if(requin.encodeur()>=110){
+     else if(requin.encodeur()>=1200){
-     requin.rotationer(0.5);
+     requin.rotationer(-0.5);
      }
      else{
-       requin.rotationer(-0.5);
+       requin.rotationer(0.5);
      }
     
   }

src/main/java/frc/robot/commands/requin/L1Requin.java

@@ -28,7 +28,7 @@ public class L1Requin extends Command {
   @Override
   public void execute() {
     
-    if(requin.encodeur()>=800 &&  requin.encodeur()<=810){
+    if(requin.encodeur()<=645 &&  requin.encodeur()>=600){
       requin.rotationer(0);
       if(requin.amp()>60){
       requin.balaye(-0.5);
@@ -38,11 +38,11 @@ public class L1Requin extends Command {
           bougie.Rouge();
       }
     }
-    else if(requin.encodeur()>=810){
+    else if(requin.encodeur()>=645){
-    requin.rotationer(0.5);
+    requin.rotationer(-0.5);
     }
     else{
-      requin.rotationer(-0.5);
+      requin.rotationer(0.5);
     }
     
     

src/main/java/frc/robot/commands/requin/aspire.java

@@ -33,7 +33,7 @@ public class aspire extends Command {
     }
      else
      {
-          requin.balaye(0.5);
+          requin.balaye(-0.5);
      }
   }
 

src/main/java/frc/robot/commands/requin/exspire.java

@@ -30,12 +30,12 @@ public class exspire extends Command {
   @Override
   public void execute() {
     if(requin.amp()> 60){
-      requin.balaye(-0.5);
+      requin.balaye(0.5);
     }
      else
      {
           bougie.Rouge();
-          requin.balaye(-0.5);
+          requin.balaye(0.5);
      }
   }
 

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

@@ -18,13 +18,13 @@ public class Elevateur extends SubsystemBase {
     private GenericEntry encodeurelevateurL2haut =
     teb.add("encodeurelevateurL2haut", -0.9).getEntry();
     private GenericEntry encodeurelevateurL3bas =
-    teb.add("encodeurelevateurL3bas", -2.9).getEntry();
+    teb.add("encodeurelevateurL3bas", -1.9).getEntry();
     private GenericEntry encodeurelevateurL3haut =
-    teb.add("encodeurelevateurL3haut", -3).getEntry();
+    teb.add("encodeurelevateurL3haut", -2.11).getEntry();
     private GenericEntry encodeurelevateurL4bas =
-    teb.add("encodeurelevateurL4bas", -6.4).getEntry();
+    teb.add("encodeurelevateurL4bas", -6.6).getEntry();
     private GenericEntry encodeurelevateurL4haut =
-    teb.add("encodeurelevateurL4haut", -6.5).getEntry();
+    teb.add("encodeurelevateurL4haut", -6.3).getEntry();
     private GenericEntry encodeurelevateurstationbas =
     teb.add("encodeurelevateursationbas", -0.5).getEntry();
     private GenericEntry encodeurelevateurstationhaut =
@@ -67,16 +67,16 @@ public class Elevateur extends SubsystemBase {
     return encodeurelevateurL2haut.getDouble(-0.9);
   }
   public double encodeurelevateurL3bas(){
-    return encodeurelevateurL3bas.getDouble(-2.9);
+    return encodeurelevateurL3bas.getDouble(-1.9);
   }
   public double encodeurelevateurL3haut(){
-    return encodeurelevateurL3haut.getDouble(-3);
+    return encodeurelevateurL3haut.getDouble(-2.11);
   }
   public double encodeurelevateurL4bas(){
-    return encodeurelevateurL4bas.getDouble(-6.4);
+    return encodeurelevateurL4bas.getDouble(-6.6);
   }
   public double encodeurelevateurL4haut(){
-    return encodeurelevateurL4haut.getDouble(-6.5);
+    return encodeurelevateurL4haut.getDouble(-6.3);
   }
   public double encodeurelevateurstationbas(){
     return encodeurelevateurstationbas.getDouble(-0.5);

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

@@ -31,7 +31,7 @@ public class Grimpeur extends SubsystemBase {
   return grimpeur.getEncoder().getPosition();
   }
   public void reset(){
-     grimpeur.getEncoder().setPosition(0);
+     grimpeur.getEncoder().setPosition(135.11);
     }
   @Override
   public void periodic() {

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

@@ -17,6 +17,8 @@ public class Pince extends SubsystemBase {
   public Pince() {
     teb.addBoolean("limit pince",this::position);
     teb.addDouble("encodeur pince", this::encodeurpivot);
+    teb.addDouble("amperage corail", this::emperagecoral);
+    teb.addDouble("amperage algue", this::emperagealgue);
   }
   final SparkMax coral = new SparkMax(20, MotorType.kBrushless);
   final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless);
@@ -30,7 +32,7 @@ public void aspirecoral(double vitesse){
 }
 public void pivote(double vitesse){
   if (position()) {
-    if (vitesse > 0) {
+    if (vitesse < 0) {
       pivoti.set(0);
     }
     else{

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

@@ -17,6 +17,8 @@ public class Requin extends SubsystemBase {
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
   public Requin() {
     teb.addBoolean("limit requin", this::enHaut);
+    teb.addDouble("amparge requin", this::amp);
+    teb.addDouble("encodeur requin", this::encodeur);
   }
    
   final SparkFlex balaye = new SparkFlex(15, MotorType.kBrushless);