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

src/main/java/frc/robot/Constants.java

@@ -1,12 +1,12 @@
 package frc.robot;
 
 public class Constants {
-    public static int avantdroit = 0;
+    public static int avantdroit = 1;
-    public static int avantgauche = 1;
+    public static int avantgauche = 4;
-    public static int arrieredroit = 2;
+    public static int arrieredroit = 3;
-    public static int arrieregauche = 3;
+    public static int arrieregauche = 5;
     public static int BrasTelescopique = 4;
-    public static int pivot = 5;
+    public static int pivot = 0;
     
     //moteur
     public static int leverGratte = 0;

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

@@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
@@ -23,6 +22,7 @@ import frc.robot.subsystems.bras.BrasTelescopique;
 import frc.robot.subsystems.bras.Pince;
 import frc.robot.subsystems.bras.Pivot;
 import frc.robot.subsystems.Limelight;
+import frc.robot.commands.Apriltag;
 // command
 import frc.robot.commands.BrakeFerme;
 import frc.robot.commands.BrakeOuvre;
@@ -30,17 +30,25 @@ import frc.robot.commands.Cone;
 import frc.robot.commands.GratteBaisser;
 import frc.robot.commands.GratteMonte;
 import frc.robot.commands.Gyro;
-import frc.robot.commands.Reculer;
 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;
+//subsystems
+import frc.robot.commands.bras.PivotChercheBas;
+import frc.robot.commands.bras.PivotChercheHaut;
+import frc.robot.commands.Cube;
+import frc.robot.commands.Tape;
 
 
 public class RobotContainer {
+<<<<<<< HEAD
   
+=======
+//CameraServer.startAutomaticCapture();
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
 CommandXboxController manette1 = new CommandXboxController(0);
 CommandXboxController manette2 = new CommandXboxController(1);
 // subsystems
@@ -49,7 +57,7 @@ Gratte gratte = new Gratte();
 BrasTelescopique brasTelescopique = new BrasTelescopique();
 Pince pince = new Pince();
 Pivot pivot = new Pivot();
-Limelight limelight = new Limelight();
+Limelight limelight = new Limelight();/** */
 //commands
 BrakeFerme brakeFerme = new BrakeFerme(basePilotable);
 BrakeOuvre brakeOuvre = new BrakeOuvre(basePilotable);
@@ -73,6 +81,12 @@ String nulpart = "nul part";
 ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto");
 GenericEntry autobalance = layoutauto.add("choix balance",true).getEntry();
 GenericEntry autosortir = layoutauto.add("choix sorit",false).getEntry();
+PivotChercheBas pivotChercheBas = new PivotChercheBas(brasTelescopique, pivot);
+PivotChercheHaut pivotChercheHaut = new PivotChercheHaut(brasTelescopique, pivot);
+Cube cube = new Cube(limelight, basePilotable, null);
+Apriltag aprilTag = new Apriltag(limelight, basePilotable, null);
+Tape tape = new Tape(limelight, basePilotable, null);
+
 public RobotContainer() {
     chooser.setDefaultOption(enhaut, enhaut);
     chooser.addOption(enbas, enbas);
@@ -82,16 +96,25 @@ public RobotContainer() {
 
 
     configureBindings();
-
+    CameraServer.startAutomaticCapture();
     basePilotable.setDefaultCommand(new RunCommand(() -> {
-      basePilotable.drive(-manette1.getLeftY(), manette1.getLeftX());
+      basePilotable.drive(-manette1.getLeftY(), manette1.getLeftX(), 0);
     },basePilotable));
     
 }
   
   private void configureBindings() {
+    basePilotable.setDefaultCommand(new RunCommand(() -> {
+      basePilotable.drive(-manette1.getLeftY(), -manette1.getLeftX(), 0);
+    },basePilotable));
+    // manette 1
+    manette1.povDown().onTrue(pivoteBrasHaut);
+    manette1.povUp().onTrue(pivoteBrasBas);
+    manette1.povLeft().onTrue(pivoteBrasMilieux);
+    manette1.povRight().onTrue(pivotBrasRentre);
     manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince));
     manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable));
+<<<<<<< HEAD
     manette1.y().whileTrue(gyro);
     manette1.start().toggleOnTrue(Commands.startEnd(basePilotable::resetGyro, basePilotable::resetGyro, basePilotable));
     /*manette1.b().toggleOnTrue(Commands.startEnd(gratte::baiser, gratte::Lever,gratte));
@@ -122,5 +145,27 @@ public RobotContainer() {
     );
 
 
+=======
+    manette1.b().onTrue(Commands.either(gratteBaisser, gratteMonte, gratte::getenHaut));
+    manette1.leftBumper().toggleOnTrue(cube);
+    manette1.rightBumper().toggleOnTrue(cone);
+    // manette 2
+    manette2.y().whileTrue(gyro);
+    manette2.start().toggleOnTrue(Commands.startEnd(basePilotable::resetGyro, basePilotable::resetGyro, basePilotable));
+    manette2.povRight().onTrue(pivotChercheBas);
+    manette2.povLeft().onTrue(pivotChercheHaut);
+    manette2.rightBumper().toggleOnTrue(aprilTag);
+    manette2.leftBumper().toggleOnTrue(tape);
+  }
+
+  public Command getAutonomousCommand() {
+    return null;
+    //return new SequentialCommandGroup(
+      //new PivoteBrasMilieux(brasTelescopique, pivot),
+      //new OuvrePince(pince),
+      //new PivotBrasRentre(brasTelescopique, pivot).alongWith(new Reculer(basePilotable)),
+      //new Gyro(basePilotable)
+    //);
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
   }
 }

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

@@ -32,7 +32,7 @@ public class Apriltag extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -32,7 +32,7 @@ public class Cone extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -32,7 +32,7 @@ public class Cube extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -18,7 +18,9 @@ public class GratteBaisser extends CommandBase {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+  gratte.setenHaut(false);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
@@ -41,9 +43,7 @@ public class GratteBaisser extends CommandBase {
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {
+  public void end(boolean interrupted) {}
-    
-  }
 
   // Returns true when the command should end.
   @Override

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

@@ -19,7 +19,9 @@ public class GratteMonte extends CommandBase {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    gratte.setenHaut(true);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override

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

@@ -4,13 +4,18 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.BasePilotable;
 
 public class Gyro extends CommandBase {
+  ShuffleboardTab teb = Shuffleboard.getTab("teb");
   private BasePilotable basePilotable;
   /** Creates a new Gyro. */
   public Gyro(BasePilotable basePilotable) {
+    teb.add("angleGyro", 0.1);
+    teb.add("vitesseGyro", 0.1);
     this.basePilotable = basePilotable;
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(basePilotable);
@@ -25,15 +30,20 @@ public class Gyro extends CommandBase {
   public void execute() {
   if(basePilotable.getpitch()>10) 
    { 
-    basePilotable.drive(0.3*basePilotable.getpitch()/15, 0);
+    basePilotable.drive(0.3*basePilotable.getpitch()/12, 0, 0);
    }
    else if(basePilotable.getpitch()<-10) 
    { 
-    basePilotable.drive(0.3*basePilotable.getpitch()/15, 0);
+    basePilotable.drive(0.3*basePilotable.getpitch()/12, 0, 0);
+    basePilotable.drive(0.3*basePilotable.getpitch()/15, 0, 0);
+   }
+   else if(basePilotable.getpitch()<-4) 
+   { 
+    basePilotable.drive(0.3*basePilotable.getpitch()/15, 0, 0);
    }
    else
    {
-     basePilotable.drive(0, 0); 
+     basePilotable.drive(0, 0, 0); 
    }
 }
 

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

@@ -27,7 +27,7 @@ public class Reculer extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(SmartDashboard.getNumber("vitesse auto", -0.3), 0);
+    basePilotable.drive(SmartDashboard.getNumber("vitesse auto", -0.3), 0, 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -32,7 +32,7 @@ public class Tape extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -7,8 +7,8 @@ package frc.robot.subsystems;
 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;
@@ -21,11 +21,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class BasePilotable extends SubsystemBase {
-  final CANSparkMax avantd = new CANSparkMax(Constants.avantdroit, MotorType.kBrushless);
+  final CANSparkMax avantdroit = new CANSparkMax(Constants.avantdroit, MotorType.kBrushless);
   final CANSparkMax avantgauche = new CANSparkMax(Constants.avantgauche, MotorType.kBrushless);
   final CANSparkMax arrieredroit = new CANSparkMax(Constants.arrieredroit, MotorType.kBrushless);
   final CANSparkMax arrieregauche = new CANSparkMax(Constants.arrieregauche, MotorType.kBrushless);
-  final MotorControllerGroup droit = new MotorControllerGroup(avantd, arrieredroit);
+  final MotorControllerGroup droit = new MotorControllerGroup(avantdroit, arrieredroit);
   final MotorControllerGroup gauche = new MotorControllerGroup(avantgauche, arrieregauche);
   final DifferentialDrive drive = new DifferentialDrive(gauche, droit);
   
@@ -35,28 +35,34 @@ public class BasePilotable extends SubsystemBase {
   //gyro
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
   ShuffleboardLayout layout = Shuffleboard.getTab("teb")
-  .getLayout ("distance", BuiltInLayouts.kList)
+  .getLayout ("encodeurs base pilotable", BuiltInLayouts.kList)
   .withSize(2, 2);
   private AHRS gyroscope = new AHRS();public double getangle() {return gyroscope.getAngle();}
    public double getpitch() {
     return gyroscope.getPitch();
   }
 
-  public void drive(double xSpeed, double zRotation){
+  public void drive(double xSpeed, double zRotation, int i){
     drive.arcadeDrive(xSpeed, zRotation);
   }
   public double distance(){
-    return (-avantd.getEncoder().getPosition()
+    teb .add ("distance",0.1);
+    return (-avantdroit.getEncoder().getPosition()
     +avantgauche.getEncoder().getPosition()
     -arrieredroit.getEncoder().getPosition()
     +arrieregauche.getEncoder().getPosition()) / 4;
   }
   public void Reset() {
-    avantd.getEncoder().setPosition(0);
+    avantdroit.getEncoder().setPosition(0);
     avantgauche.getEncoder().setPosition(0);
     arrieredroit.getEncoder().setPosition(0);
     arrieregauche.getEncoder().setPosition(0);
   }
+  public void resetGyro(){
+    {gyroscope.reset();
+    }
+  } 
+
 public void BrakeOuvre(){
   brakedroit.set(Value.kForward);
   brakegauche.set(Value.kForward);
@@ -65,13 +71,9 @@ public void BrakeFerme(){
   brakedroit.set(Value.kReverse);
   brakegauche.set(Value.kReverse);
 }
-public void resetGyro(){
-  {gyroscope.reset();} 
- }
   /** Creates a new BasePilotable. */
   public BasePilotable() {
     droit.setInverted(true);
-    layout .addDouble("distance", this::distance);
   }
 
   @Override

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

@@ -3,7 +3,6 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot.subsystems;
-
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import edu.wpi.first.wpilibj.DigitalInput;
@@ -14,7 +13,11 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.robot.Constants;
 
 public class Gratte extends SubsystemBase {
+<<<<<<< HEAD
  ShuffleboardTab teb = Shuffleboard.getTab("teb"); 
+=======
+ShuffleboardTab teb = Shuffleboard.getTab("teb"); 
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
 ShuffleboardLayout limitswitchgratte = Shuffleboard.getTab("teb")
 .getLayout("limitswitchsgratte", BuiltInLayouts.kList)
 .withSize(2, 2);
@@ -24,7 +27,15 @@ ShuffleboardLayout limitswitchgratte = Shuffleboard.getTab("teb")
   private DigitalInput limithg = new DigitalInput(Constants.limithg);
   private DigitalInput limitbd = new DigitalInput(Constants.limitbd);
   private DigitalInput limitbg = new DigitalInput(Constants.limitbg);
-public boolean baiser;
+  public boolean baiser;
+  boolean enHaut = true;
+  public void setenHaut(boolean enHaut){
+    this.enHaut = enHaut;
+  }
+  public boolean getenHaut(){
+    return enHaut;
+    
+  }
 
   public boolean hautd(){
     return limithd.get();
@@ -34,14 +45,19 @@ public boolean baiser;
     return limithg.get();
   }
 
-  public boolean basd(int i, String string){
+  public boolean basd(){
     return limitbd.get();
   }
   public boolean basg(){
     return limitbg.get();
   }
-  /** Creates a new Gratte. */
+ 
-  public Gratte() {}
+  public Gratte() { 
+    limitswitchgratte.add ("limitbd", 0.1);
+    limitswitchgratte.add ("limithg", 0.1);
+    limitswitchgratte.add ("limithd", 0.1);
+    limitswitchgratte.add ("limitbg", 0.1);
+  }
   public void Lever(double vitesse){
     Gratted.set(vitesse);
     Gratteg.set(vitesse);
@@ -51,18 +67,6 @@ public boolean baiser;
     Gratteg.set(-vitesse);
   }
   @Override
-  public void periodic() {
+  public void periodic(){
-    teb .add("limithd", 0.1);
-    teb .add("limithg", 0.1);
-    teb .add("limitbd", 0.1);
-    teb .add("limitbg", 0.1);
-    limitswitchgratte.add ("limitbd", 0.1);
-    limitswitchgratte.add ("limithg", 0.1);
-    limitswitchgratte.add ("limithd", 0.1);
-    limitswitchgratte.add ("limitbg", 0.1);
  }
-
-public boolean basd() {
-    return false;
-}
 }

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

@@ -21,6 +21,8 @@ public class Limelight extends SubsystemBase {
   PhotonCamera limelight = new PhotonCamera("limelight");
   /** Creates a new Limelight. */
   public Limelight() {
+    CameraServer.startAutomaticCapture();
+    teb .add("limelight", 0.1);
     PortForwarder.add(5800, "photonvision.local", 5800);
   }
 
@@ -58,7 +60,5 @@ public class Limelight extends SubsystemBase {
   }
   @Override
   public void periodic() {
-    CameraServer.startAutomaticCapture();
-    teb .add("limelight", 0.1);
   }
 }

src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java

@@ -12,14 +12,13 @@ import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class BrasTelescopique extends SubsystemBase {
+<<<<<<< HEAD
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
   ShuffleboardLayout layout = Shuffleboard.getTab("teb")
   .getLayout("layout", BuiltInLayouts.kList)
@@ -28,8 +27,14 @@ public class BrasTelescopique extends SubsystemBase {
 ShuffleboardLayout bras = Shuffleboard.getTab("teb")
 .getLayout("bras", BuiltInLayouts.kList)
 .withSize(2, 2);
+=======
+  ShuffleboardTab teb = Shuffleboard.getTab("teb");  
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
   /** Creates a new BrasTelescopique. */
-  public BrasTelescopique() {}
+  public BrasTelescopique() {
+    teb .add("photocell",0.1);
+    teb .add("winch",0.1);
+    teb .add("encodeur",0.1);}
   final CANSparkMax Winch = new CANSparkMax(Constants.BrasTelescopique,MotorType.kBrushless);
   private DigitalInput photocell = new DigitalInput(Constants.photocell);
   private DoubleSolenoid brakewinch = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakewinchf, Constants.brakewinchb);
@@ -53,8 +58,6 @@ ShuffleboardLayout bras = Shuffleboard.getTab("teb")
     }
   @Override
   public void periodic() {
-    teb .add("photocell",0.1);
+    
-    teb .add("winch",0.1);
-    bras.add ("encodeur",0.1);
   }
 }

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

@@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
-
 import frc.robot.Constants;
 
 

src/main/java/frc/robot/subsystems/bras/Pivot.java

@@ -14,7 +14,12 @@ import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 public class Pivot extends SubsystemBase {
+<<<<<<< HEAD
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
+=======
+ ShuffleboardTab teb = Shuffleboard.getTab("teb");
+  
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
   // moteur
   private CANSparkMax pivot = new CANSparkMax(Constants.pivot,MotorType.kBrushless);
   private DigitalInput limitpivot = new DigitalInput(Constants.limitpivot);
@@ -23,6 +28,10 @@ public class Pivot extends SubsystemBase {
   public void monteDescendre(double vitesse) {
     pivot.set (vitesse);
   } 
+  public Pivot(){
+    teb .add ("encodeurpivot",0.1);
+    teb .add ("limitpivot",0.1);
+  }
   // encodeur
   public double distance(){
     return (pivot.getEncoder().getPosition());
@@ -36,9 +45,8 @@ public class Pivot extends SubsystemBase {
   }
   @Override
   public void periodic() {
-    teb .add("encodeur", 0.1);
-    teb .add ("encodeur pivot",0.1);
   }
+<<<<<<< HEAD
 
   
  
@@ -47,3 +55,7 @@ public class Pivot extends SubsystemBase {
   teb.add ("encodeur pivot",0.1);
  }
 }
+=======
+}  
+
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e